MRPT  2.0.1
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;
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
float getXMax() const
Returns the "x" coordinate of right side of grid map.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Parameters for CMetricMap::compute3DMatchingRatio()
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:42
void colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:114
float getYMin() const
Returns the "y" coordinate of top side of grid map.
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:54
void getMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD) const
Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods)...
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
double & dm_mean()
[Kernel-methods only] The cumulative weighted readings of this cell
double evaluateResidual() const override
Return the residual/error of this observation.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
double & kf_std()
[KF-methods only] The standard deviation value of this cell
void resize(size_t row, size_t col)
double & dm_mean_w()
[Kernel-methods only] The cumulative weights (concentration = alpha
virtual void saveAsMatlab3DGraph(const std::string &filName) const
Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the...
void updateEstimation(mrpt::math::CVectorDouble &solved_x_inc, mrpt::math::CVectorDouble *solved_variances=nullptr)
float getResolution() const
Returns the resolution of the grid map.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
float KF_defaultCellMeanValue
The default value for the mean of cells&#39; concentration.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
bool eraseConstraint(const FactorBase &c)
Removes a constraint.
double getInformation() const override
Return the inverse of the variance of this constraint.
const float & x(size_t i) const
Definition: TTriangle.h:90
void fill(const Scalar &val)
bool exist_relation_between2cells(const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap, size_t cxo_min, size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo, const size_t seed_cyo, const size_t objective_cxo, const size_t objective_cyo)
Check if two cells of the gridmap (m_map) are connected, based on the provided occupancy gridmap...
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
void insertObservation_GMRF(double normReading, const mrpt::math::TPoint2D &point, const bool update_map, const bool time_invariant, const double reading_information)
The implementation of "insertObservation" for the Gaussian Markov Random Field map model...
A triangle (float coordinates) with RGBA colors (u8) and UV (texture coordinates) for each vertex...
Definition: TTriangle.h:35
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
std::vector< std::list< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information.
A high-performance stopwatch, with typical resolution of nanoseconds.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
double Lambda
"Information" of the observation (=inverse of the variance)
mrpt::vision::TStereoCalibParams params
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
void idx2cxcy(int idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.
Definition: CDynamicGrid.h:271
void getMeanAndCov(mrpt::math::CVectorDouble &out_means, mrpt::math::CMatrixDouble &out_cov) const
Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based meth...
bool loadFromBitmapFile(const std::string &file, float resolution, const mrpt::math::TPoint2D &origin=mrpt::math::TPoint2D(std::numeric_limits< double >::max(), std::numeric_limits< double >::max()))
Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
STL namespace.
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:274
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:279
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
std::vector< TRandomFieldCell > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
Definition: CDynamicGrid.h:45
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed...
virtual bool getEdgeInformation(const CRandomFieldGridMap2D *parent, size_t icx, size_t icy, size_t jcx, size_t jcy, double &out_edge_information)=0
Implement the check of whether node i=(icx,icy) is connected with node j=(jcx,jcy).
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells&#39; mean and std ...
void internal_dumpToTextStream_common(std::ostream &out) const
See utils::CLoadableOptions.
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
mrpt::graphs::ScalarFactorGraph m_gmrf
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
const uint8_t & a(size_t i) const
Definition: TTriangle.h:96
void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell *fill_value=nullptr)
Changes the size of the grid, ERASING all previous contents.
Definition: CDynamicGrid.h:74
void saturate(T &var, const T sat_min, const T sat_max)
Saturate the value of var (the variable gets modified) so it does not get out of [min,max].
std::array< Vertex, 3 > vertices
Definition: TTriangle.h:88
double getInformation() const override
Return the inverse of the variance of this constraint.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
~CRandomFieldGridMap2D() override
Destructor.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false) ...
bool saveAsBitmapFile(const std::string &file) const
Saves the gridmap as a graphical file (BMP,PNG,...).
float R_min
Limits for normalization of sensor readings.
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
This class allows loading and storing values and vectors of different types from a configuration text...
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
This base provides a set of functions for maths stuff.
double & kf_mean()
[KF-methods only] The mean value of this cell
void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const TRandomFieldCell &defaultValueNewCells, double additionalMarginMeters=1.0f) override
Changes the size of the grid, maintaining previous contents.
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell ...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
float getXMin() const
Returns the "x" coordinate of left side of grid map.
The contents of each cell in a CRandomFieldGridMap2D map.
void setMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD)
Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods)...
Versatile class for consistent logging and management of output messages.
TRandomFieldCell * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:201
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell *fill_value=nullptr)
Changes the size of the grid, erasing previous contents.
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
int val
Definition: mrpt_jpeglib.h:957
void insertObservation_KernelDM_DMV(double normReading, const mrpt::math::TPoint2D &point, bool is_DMV)
The implementation of "insertObservation" for Achim Lilienthal&#39;s map models DM & DM+V.
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
void setFromMatrix(const MAT &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: img/CImage.h:844
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
TRandomFieldCell * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:222
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
virtual void getAsBitmapFile(mrpt::img::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
void internal_loadFromConfigFile_common(const mrpt::config::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
void setCellsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
return_t square(const num_t x)
Inline function for the square of a number.
fixed floating point &#39;f&#39;
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
double GMRF_saturate_min
(Default:-inf,+inf) Saturate the estimated mean in these limits
A class for storing an occupancy grid map.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float & y(size_t i) const
Definition: TTriangle.h:91
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
double & gmrf_mean()
[GMRF only] The mean value of this cell
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
The implementation in this class just calls all the corresponding method of the contained metric maps...
const float & z(size_t i) const
Definition: TTriangle.h:92
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
void internal_clear() override
Erase all the contents of the map.
#define MRPT_END
Definition: exceptions.h:245
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:256
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
virtual void predictMeasurement(const double x, const double y, double &out_predict_response, double &out_predict_response_variance, bool do_sensor_normalization, const TGridInterpolationMethod interp_method=gimNearest)
Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form o...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:330
double evaluateResidual() const override
Return the residual/error of this observation.
double Lambda
"Information" of the observation (=inverse of the variance)
void resize(std::size_t N, bool zeroNewElements=false)
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
T saturate_val(const T &value, const T sat_min, const T sat_max)
Like saturate() but it returns the value instead of modifying the variable.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
images resize(NUM_IMGS)
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
!
Definition: CMetricMap.h:106
float KF_initialCellStd
The initial standard deviation of each cell&#39;s concentration (will be stored both at each cell&#39;s struc...
virtual void getAsMatrix(mrpt::math::CMatrixDouble &out_mat) const
Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y...
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
void insertIndividualReading(const double sensorReading, const mrpt::math::TPoint2D &point, const bool update_map=true, const bool time_invariant=true, const double reading_stddev=.0)
Direct update of the map with a reading in a given position of the map, using the appropriate method ...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
See docs in base class: in this class this always returns 0.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020