MRPT  1.9.9
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-2019, 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  ---------------------------------------------------------------*/
688  internal_dumpToTextStream_common(std::ostream& out) const
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  ---------------------------------------------------------------*/
791 {
792  MRPT_START
793 
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:
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  // ----------------------------------
1630  std::make_shared<opengl::CSetOfTriangles>();
1631  obj_m->enableTransparency(true);
1633  std::make_shared<opengl::CSetOfTriangles>();
1634  obj_v->enableTransparency(true);
1635 
1636  // Compute mean max/min values:
1637  // ---------------------------------------
1638  double maxVal_m = 0, minVal_m = 1, AMaxMin_m, maxVal_v = 0,
1639  minVal_v = 1, AMaxMin_v;
1640  double c, v;
1641  for (cy = 1; cy < m_size_y; cy++)
1642  {
1643  for (cx = 1; cx < m_size_x; cx++)
1644  {
1645  const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
1646  ASSERT_(cell_xy != nullptr);
1647  // mean
1648  c = cell_xy->kf_mean;
1649  minVal_m = min(minVal_m, c);
1650  maxVal_m = max(maxVal_m, c);
1651  // variance
1652  v = square(cell_xy->kf_std);
1653  minVal_v = min(minVal_v, v);
1654  maxVal_v = max(maxVal_v, v);
1655  }
1656  }
1657 
1658  AMaxMin_m = maxVal_m - minVal_m;
1659  if (AMaxMin_m == 0) AMaxMin_m = 1;
1660  AMaxMin_v = maxVal_v - minVal_v;
1661  if (AMaxMin_v == 0) AMaxMin_v = 1;
1662 
1663  // ---------------------------------------
1664  // Compute Maps
1665  // ---------------------------------------
1666  triag.a[0] = triag.a[1] = triag.a[2] =
1667  0.75f; // alpha (transparency)
1668  for (cy = 1; cy < m_size_y; cy++)
1669  {
1670  for (cx = 1; cx < m_size_x; cx++)
1671  {
1672  // Cell values:
1673  const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
1674  ASSERT_(cell_xy != nullptr);
1675  const TRandomFieldCell* cell_x_1y = cellByIndex(cx - 1, cy);
1676  ASSERT_(cell_x_1y != nullptr);
1677  const TRandomFieldCell* cell_xy_1 = cellByIndex(cx, cy - 1);
1678  ASSERT_(cell_xy_1 != nullptr);
1679  const TRandomFieldCell* cell_x_1y_1 =
1680  cellByIndex(cx - 1, cy - 1);
1681  ASSERT_(cell_x_1y_1 != nullptr);
1682 
1683  // MEAN values
1684  //-----------------
1685  double c_xy = mrpt::saturate_val(
1686  cell_xy->kf_mean,
1689  double c_x_1y = mrpt::saturate_val(
1690  cell_x_1y->kf_mean,
1693  double c_xy_1 = mrpt::saturate_val(
1694  cell_xy_1->kf_mean,
1697  double c_x_1y_1 = mrpt::saturate_val(
1698  cell_x_1y_1->kf_mean,
1701 
1702  double col_xy = c_xy;
1703  double col_x_1y = c_x_1y;
1704  double col_xy_1 = c_xy_1;
1705  double col_x_1y_1 = c_x_1y_1;
1706 
1707  // Triangle #1:
1708  triag.x[0] = xs[cx];
1709  triag.y[0] = ys[cy];
1710  triag.z[0] = c_xy;
1711  triag.x[1] = xs[cx];
1712  triag.y[1] = ys[cy - 1];
1713  triag.z[1] = c_xy_1;
1714  triag.x[2] = xs[cx - 1];
1715  triag.y[2] = ys[cy - 1];
1716  triag.z[2] = c_x_1y_1;
1717  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1718  jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
1719  jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);
1720  obj_m->insertTriangle(triag);
1721 
1722  // Triangle #2:
1723  triag.x[0] = xs[cx];
1724  triag.y[0] = ys[cy];
1725  triag.z[0] = c_xy;
1726  triag.x[1] = xs[cx - 1];
1727  triag.y[1] = ys[cy - 1];
1728  triag.z[1] = c_x_1y_1;
1729  triag.x[2] = xs[cx - 1];
1730  triag.y[2] = ys[cy];
1731  triag.z[2] = c_x_1y;
1732  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1733  jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
1734  jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
1735  obj_m->insertTriangle(triag);
1736 
1737  // VARIANCE values
1738  //------------------
1739  double v_xy = min(10.0, max(0.0, square(cell_xy->kf_std)));
1740  double v_x_1y =
1741  min(10.0, max(0.0, square(cell_x_1y->kf_std)));
1742  double v_xy_1 =
1743  min(10.0, max(0.0, square(cell_xy_1->kf_std)));
1744  double v_x_1y_1 =
1745  min(10.0, max(0.0, square(cell_x_1y_1->kf_std)));
1746 
1747  col_xy =
1748  v_xy /
1749  10; // min(1.0,max(0.0, (v_xy-minVal_v)/AMaxMin_v ) );
1750  col_x_1y = v_x_1y / 10; // min(1.0,max(0.0,
1751  // (v_x_1y-minVal_v)/AMaxMin_v ) );
1752  col_xy_1 = v_xy_1 / 10; // min(1.0,max(0.0,
1753  // (v_xy_1-minVal_v)/AMaxMin_v ) );
1754  col_x_1y_1 = v_x_1y_1 / 10; // min(1.0,max(0.0,
1755  // (v_x_1y_1-minVal_v)/AMaxMin_v ) );
1756 
1757  // Triangle #1:
1758  triag.x[0] = xs[cx];
1759  triag.y[0] = ys[cy];
1760  triag.z[0] = c_xy + v_xy;
1761  triag.x[1] = xs[cx];
1762  triag.y[1] = ys[cy - 1];
1763  triag.z[1] = c_xy_1 + v_xy_1;
1764  triag.x[2] = xs[cx - 1];
1765  triag.y[2] = ys[cy - 1];
1766  triag.z[2] = c_x_1y_1 + v_x_1y_1;
1767  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1768  jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
1769  jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);
1770  obj_v->insertTriangle(triag);
1771 
1772  // Triangle #2:
1773  triag.x[0] = xs[cx];
1774  triag.y[0] = ys[cy];
1775  triag.z[0] = c_xy + v_xy;
1776  triag.x[1] = xs[cx - 1];
1777  triag.y[1] = ys[cy - 1];
1778  triag.z[1] = c_x_1y_1 + v_x_1y_1;
1779  triag.x[2] = xs[cx - 1];
1780  triag.y[2] = ys[cy];
1781  triag.z[2] = c_x_1y + v_x_1y;
1782  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1783  jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
1784  jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
1785  obj_v->insertTriangle(triag);
1786 
1787  } // for cx
1788  } // for cy
1789  meanObj->insert(obj_m);
1790  varObj->insert(obj_v);
1791  }
1792  break; // end KF models
1793 
1794  case mrKernelDM:
1795  case mrKernelDMV:
1796  {
1797  // Draw for Kernel model:
1798  // ----------------------------------
1800  std::make_shared<opengl::CSetOfTriangles>();
1801  obj_m->enableTransparency(true);
1803  std::make_shared<opengl::CSetOfTriangles>();
1804  obj_v->enableTransparency(true);
1805 
1806  // Compute mean max/min values:
1807  // ---------------------------------------
1808  double maxVal_m = 0, minVal_m = 1, AMaxMin_m, maxVal_v = 0,
1809  minVal_v = 1, AMaxMin_v;
1810  double c, v;
1811  for (cy = 1; cy < m_size_y; cy++)
1812  {
1813  for (cx = 1; cx < m_size_x; cx++)
1814  {
1815  const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
1816  ASSERT_(cell_xy != nullptr);
1817  // mean
1818  c = computeMeanCellValue_DM_DMV(cell_xy);
1819  minVal_m = min(minVal_m, c);
1820  maxVal_m = max(maxVal_m, c);
1821  // variance
1822  v = computeVarCellValue_DM_DMV(cell_xy);
1823  minVal_v = min(minVal_v, v);
1824  maxVal_v = max(maxVal_v, v);
1825  }
1826  }
1827 
1828  AMaxMin_m = maxVal_m - minVal_m;
1829  if (AMaxMin_m == 0) AMaxMin_m = 1;
1830  AMaxMin_v = maxVal_v - minVal_v;
1831  if (AMaxMin_v == 0) AMaxMin_v = 1;
1832 
1833  // ---------------------------------------
1834  // Compute Maps
1835  // ---------------------------------------
1836  triag.a[0] = triag.a[1] = triag.a[2] =
1837  0.75f; // alpha (transparency)
1838  for (cy = 1; cy < m_size_y; cy++)
1839  {
1840  for (cx = 1; cx < m_size_x; cx++)
1841  {
1842  // Cell values:
1843  const TRandomFieldCell* cell_xy = cellByIndex(cx, cy);
1844  ASSERT_(cell_xy != nullptr);
1845  const TRandomFieldCell* cell_x_1y = cellByIndex(cx - 1, cy);
1846  ASSERT_(cell_x_1y != nullptr);
1847  const TRandomFieldCell* cell_xy_1 = cellByIndex(cx, cy - 1);
1848  ASSERT_(cell_xy_1 != nullptr);
1849  const TRandomFieldCell* cell_x_1y_1 =
1850  cellByIndex(cx - 1, cy - 1);
1851  ASSERT_(cell_x_1y_1 != nullptr);
1852 
1853  // MEAN values
1854  //-----------------
1855  double c_xy = mrpt::saturate_val(
1856  computeMeanCellValue_DM_DMV(cell_xy),
1859  double c_x_1y = mrpt::saturate_val(
1860  computeMeanCellValue_DM_DMV(cell_x_1y),
1863  double c_xy_1 = mrpt::saturate_val(
1864  computeMeanCellValue_DM_DMV(cell_xy_1),
1867  double c_x_1y_1 = mrpt::saturate_val(
1868  computeMeanCellValue_DM_DMV(cell_x_1y_1),
1871 
1872  double col_xy = c_xy;
1873  double col_x_1y = c_x_1y;
1874  double col_xy_1 = c_xy_1;
1875  double col_x_1y_1 = c_x_1y_1;
1876 
1877  // Triangle #1:
1878  triag.x[0] = xs[cx];
1879  triag.y[0] = ys[cy];
1880  triag.z[0] = c_xy;
1881  triag.x[1] = xs[cx];
1882  triag.y[1] = ys[cy - 1];
1883  triag.z[1] = c_xy_1;
1884  triag.x[2] = xs[cx - 1];
1885  triag.y[2] = ys[cy - 1];
1886  triag.z[2] = c_x_1y_1;
1887  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1888  jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
1889  jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);
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  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1904  jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
1905  jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
1906  obj_m->insertTriangle(triag);
1907 
1908  // VARIANCE values
1909  //------------------
1910  double v_xy =
1911  min(1.0, max(0.0, computeVarCellValue_DM_DMV(cell_xy)));
1912  double v_x_1y = min(
1913  1.0, max(0.0, computeVarCellValue_DM_DMV(cell_x_1y)));
1914  double v_xy_1 = min(
1915  1.0, max(0.0, computeVarCellValue_DM_DMV(cell_xy_1)));
1916  double v_x_1y_1 = min(
1917  1.0, max(0.0, computeVarCellValue_DM_DMV(cell_x_1y_1)));
1918 
1919  col_xy = v_xy;
1920  col_x_1y = v_x_1y;
1921  col_xy_1 = v_xy_1;
1922  col_x_1y_1 = v_x_1y_1;
1923 
1924  // Triangle #1:
1925  triag.x[0] = xs[cx];
1926  triag.y[0] = ys[cy];
1927  triag.z[0] = v_xy;
1928  triag.x[1] = xs[cx];
1929  triag.y[1] = ys[cy - 1];
1930  triag.z[1] = v_xy_1;
1931  triag.x[2] = xs[cx - 1];
1932  triag.y[2] = ys[cy - 1];
1933  triag.z[2] = v_x_1y_1;
1934  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1935  jet2rgb(col_xy_1, triag.r[1], triag.g[1], triag.b[1]);
1936  jet2rgb(col_x_1y_1, triag.r[2], triag.g[2], triag.b[2]);
1937 
1938  obj_v->insertTriangle(triag);
1939 
1940  // Triangle #2:
1941  triag.x[0] = xs[cx];
1942  triag.y[0] = ys[cy];
1943  triag.z[0] = v_xy;
1944  triag.x[1] = xs[cx - 1];
1945  triag.y[1] = ys[cy - 1];
1946  triag.z[1] = v_x_1y_1;
1947  triag.x[2] = xs[cx - 1];
1948  triag.y[2] = ys[cy];
1949  triag.z[2] = v_x_1y;
1950  jet2rgb(col_xy, triag.r[0], triag.g[0], triag.b[0]);
1951  jet2rgb(col_x_1y_1, triag.r[1], triag.g[1], triag.b[1]);
1952  jet2rgb(col_x_1y, triag.r[2], triag.g[2], triag.b[2]);
1953  obj_v->insertTriangle(triag);
1954 
1955  } // for cx
1956  } // for cy
1957  meanObj->insert(obj_m);
1958  varObj->insert(obj_v);
1959  }
1960  break; // end Kernel models
1961  }; // end switch maptype
1962 }
1963 
1964 /*---------------------------------------------------------------
1965  computeConfidenceCellValue_DM_DMV
1966  ---------------------------------------------------------------*/
1968  const TRandomFieldCell* cell) const
1969 {
1970  // A confidence measure:
1971  return 1.0 - std::exp(-square(
1973 }
1974 
1975 /*---------------------------------------------------------------
1976  computeMeanCellValue_DM_DMV
1977  ---------------------------------------------------------------*/
1979  const TRandomFieldCell* cell) const
1980 {
1981  // A confidence measure:
1982  const double alpha =
1983  1.0 - std::exp(-square(
1985  const double r_val =
1986  (cell->dm_mean_w > 0) ? (cell->dm_mean / cell->dm_mean_w) : 0;
1987  return alpha * r_val + (1 - alpha) * m_average_normreadings_mean;
1988 }
1989 
1990 /*---------------------------------------------------------------
1991  computeVarCellValue_DM_DMV
1992  ---------------------------------------------------------------*/
1994  const TRandomFieldCell* cell) const
1995 {
1996  // A confidence measure:
1997  const double alpha =
1998  1.0 - std::exp(-square(
2000  const double r_val =
2001  (cell->dm_mean_w > 0) ? (cell->dmv_var_mean / cell->dm_mean_w) : 0;
2002  return alpha * r_val + (1 - alpha) * m_average_normreadings_var;
2003 }
2004 
2006 {
2007  int cx, cy;
2008  double val, var;
2009  double coef;
2010 };
2011 
2012 /*---------------------------------------------------------------
2013  predictMeasurement
2014  ---------------------------------------------------------------*/
2016  const double x, const double y, double& out_predict_response,
2017  double& out_predict_response_variance, bool do_sensor_normalization,
2018  const TGridInterpolationMethod interp_method)
2019 {
2020  MRPT_START
2021 
2022  vector<TInterpQuery> queries;
2023  switch (interp_method)
2024  {
2025  case gimNearest:
2026  queries.resize(1);
2027  queries[0].cx = x2idx(x);
2028  queries[0].cy = y2idx(y);
2029  queries[0].coef = 1.0;
2030  break;
2031 
2032  case gimBilinear:
2033  if (x <= m_x_min + m_resolution * 0.5 ||
2034  y <= m_y_min + m_resolution * 0.5 ||
2035  x >= m_x_max - m_resolution * 0.5 ||
2036  x >= m_x_max - m_resolution * 0.5)
2037  {
2038  // Too close to a border:
2039  queries.resize(1);
2040  queries[0].cx = x2idx(x);
2041  queries[0].cy = y2idx(y);
2042  queries[0].coef = 1.0;
2043  }
2044  else
2045  {
2046  queries.resize(4);
2047  const double K_1 = 1.0 / (m_resolution * m_resolution);
2048  // 11
2049  queries[0].cx = x2idx(x - m_resolution * 0.5);
2050  queries[0].cy = y2idx(y - m_resolution * 0.5);
2051  // 12
2052  queries[1].cx = x2idx(x - m_resolution * 0.5);
2053  queries[1].cy = y2idx(y + m_resolution * 0.5);
2054  // 21
2055  queries[2].cx = x2idx(x + m_resolution * 0.5);
2056  queries[2].cy = y2idx(y - m_resolution * 0.5);
2057  // 22
2058  queries[3].cx = x2idx(x + m_resolution * 0.5);
2059  queries[3].cy = y2idx(y + m_resolution * 0.5);
2060  // Weights:
2061  queries[0].coef = K_1 * (idx2x(queries[3].cx) - x) *
2062  (idx2y(queries[3].cy) - y);
2063  queries[1].coef = K_1 * (idx2x(queries[3].cx) - x) *
2064  (y - idx2y(queries[0].cy));
2065  queries[2].coef = K_1 * (x - idx2x(queries[0].cx)) *
2066  (idx2y(queries[3].cy) - y);
2067  queries[3].coef = K_1 * (x - idx2x(queries[0].cx)) *
2068  (y - idx2y(queries[0].cy));
2069  }
2070  break;
2071  default:
2072  THROW_EXCEPTION("Unknown interpolation method!");
2073  };
2074 
2075  // Run queries:
2076  for (auto& q : queries)
2077  {
2078  const TRandomFieldCell* cell = cellByIndex(q.cx, q.cy);
2079  switch (m_mapType)
2080  {
2081  case mrKernelDM:
2082  {
2083  if (!cell)
2084  {
2087  }
2088  else
2089  {
2090  q.val = computeMeanCellValue_DM_DMV(cell);
2092  }
2093  }
2094  break;
2095  case mrKernelDMV:
2096  {
2097  if (!cell)
2098  {
2101  }
2102  else
2103  {
2104  q.val = computeMeanCellValue_DM_DMV(cell);
2105  q.var = computeVarCellValue_DM_DMV(cell);
2106  }
2107  }
2108  break;
2109 
2110  case mrKalmanFilter:
2111  case mrKalmanApproximate:
2112  case mrGMRF_SD:
2113  {
2114  if (m_mapType == mrKalmanApproximate &&
2116  recoverMeanAndCov(); // Just for KF2
2117 
2118  if (!cell)
2119  {
2121  q.var =
2123  square(
2125  }
2126  else
2127  {
2128  q.val = cell->kf_mean;
2129  q.var =
2130  square(cell->kf_std) +
2131  square(
2133  }
2134  }
2135  break;
2136 
2137  default:
2138  THROW_EXCEPTION("Invalid map type.");
2139  };
2140  }
2141 
2142  // Sum coeffs:
2143  out_predict_response = 0;
2144  out_predict_response_variance = 0;
2145  for (auto& querie : queries)
2146  {
2147  out_predict_response += querie.val * querie.coef;
2148  out_predict_response_variance += querie.var * querie.coef;
2149  }
2150 
2151  // Un-do the sensor normalization:
2152  if (do_sensor_normalization)
2153  out_predict_response =
2155  out_predict_response *
2157 
2158  MRPT_END
2159 }
2160 
2161 /*---------------------------------------------------------------
2162  insertObservation_KF2
2163  ---------------------------------------------------------------*/
2165  double normReading, const mrpt::math::TPoint2D& point)
2166 {
2167  MRPT_START
2168 
2170  "Inserting KF2: (" << normReading << ") at Position" << point << endl);
2171 
2172  const signed W = m_insertOptions_common->KF_W_size;
2173  const size_t K = 2 * W * (W + 1) + 1;
2174  const size_t W21 = 2 * W + 1;
2175  const size_t W21sqr = W21 * W21;
2176 
2177  ASSERT_(W >= 2);
2178 
2179  m_hasToRecoverMeanAndCov = true;
2180 
2181  const TRandomFieldCell defCell(
2183  -1 // Just to indicate that cells are new, next changed to:
2184  // m_insertOptions_common->KF_initialCellStd // std
2185  );
2186 
2187  // Assure we have room enough in the grid!
2188  const double Aspace = (W + 1) * m_resolution;
2189 
2190  resize(
2191  point.x - Aspace, point.x + Aspace, point.y - Aspace, point.y + Aspace,
2192  defCell, Aspace);
2193 
2194  // --------------------------------------------------------
2195  // The Kalman-Filter estimation of the MRF grid-map:
2196  // --------------------------------------------------------
2197  const size_t N = m_map.size();
2198 
2199  ASSERT_(int(K) == m_stackedCov.cols());
2200  ASSERT_(int(N) == m_stackedCov.rows());
2201 
2202  // Prediction stage of KF:
2203  // ------------------------------------
2204  // Nothing to do here (static map)
2205 
2206  // Update stage of KF:
2207  // We directly apply optimized formulas arising
2208  // from our concrete sensor model.
2209  // -------------------------------------------------
2210 
2211  // const double KF_covSigma2 = square(m_insertOptions_common->KF_covSigma);
2212  // const double std0 = m_insertOptions_common->KF_initialCellStd;
2213  // const double res2 = square(m_resolution);
2214  const int cellIdx = xy2idx(point.x, point.y);
2215  TRandomFieldCell* cell = cellByPos(point.x, point.y);
2216  ASSERT_(cell != nullptr);
2217 
2218  // Predicted observation mean
2219  double yk = normReading - cell->kf_mean;
2220 
2221  // Predicted observation variance
2222  double sk = m_stackedCov(cellIdx, 0) + // Variance of that cell: cov(i,i)
2224 
2225  double sk_1 = 1.0 / sk;
2226 
2227  static CTicTac tictac;
2228  MRPT_LOG_DEBUG("[insertObservation_KF2] Updating mean values...");
2229  tictac.Tic();
2230 
2231  // ------------------------------------------------------------
2232  // Update mean values:
2233  // Process only those cells in the vecinity of the cell (c):
2234  //
2235  // What follows is *** REALLY UGLY *** for efficiency, sorry!! :-)
2236  // ------------------------------------------------------------
2237  const int cx_c = x2idx(point.x);
2238  const int cy_c = y2idx(point.y);
2239 
2240  const int Acx0 = max(-W, -cx_c);
2241  const int Acy0 = max(-W, -cy_c);
2242  const int Acx1 = min(W, int(m_size_x) - 1 - cx_c);
2243  const int Acy1 = min(W, int(m_size_y) - 1 - cy_c);
2244 
2245  // We will fill this now, so we already have it for updating the
2246  // covariances next:
2247  CVectorDouble cross_covs_c_i(W21sqr, 0); // Indexes are relative to the
2248  // (2W+1)x(2W+1) window centered
2249  // at "cellIdx".
2250  std::vector<int> window_idxs(W21sqr, -1); // The real-map indexes for each
2251  // element in the window, or -1 if
2252  // there are out of the map (for cells
2253  // close to the border)
2254 
2255  // 1) First, the cells before "c":
2256  for (int Acy = Acy0; Acy <= 0; Acy++)
2257  {
2258  int limit_cx = Acy < 0 ? Acx1 : -1;
2259 
2260  size_t idx = cx_c + Acx0 + m_size_x * (cy_c + Acy);
2261  int idx_c_in_idx = -Acy * W21 - Acx0;
2262 
2263  int window_idx = Acx0 + W + (Acy + W) * W21;
2264 
2265  for (int Acx = Acx0; Acx <= limit_cx; Acx++)
2266  {
2267  ASSERT_(idx_c_in_idx > 0);
2268  const double cov_i_c = m_stackedCov(idx, idx_c_in_idx);
2269  // JGmonroy - review m_stackedCov
2270 
2271  m_map[idx].kf_mean += yk * sk_1 * cov_i_c;
2272 
2273  // Save window indexes:
2274  cross_covs_c_i[window_idx] = cov_i_c;
2275  window_idxs[window_idx++] = idx;
2276 
2277  idx_c_in_idx--;
2278  idx++;
2279  }
2280  }
2281 
2282  // 2) The cell "c" itself, and the rest within the window:
2283  for (int Acy = 0; Acy <= Acy1; Acy++)
2284  {
2285  int start_cx = Acy > 0 ? Acx0 : 0;
2286 
2287  size_t idx = cx_c + start_cx + m_size_x * (cy_c + Acy);
2288  int idx_i_in_c;
2289  if (Acy > 0)
2290  idx_i_in_c =
2291  (W + 1) + (Acy - 1) * W21 + (start_cx + W); // Rest of rows
2292  else
2293  idx_i_in_c = 0; // First row.
2294 
2295  int window_idx = start_cx + W + (Acy + W) * W21;
2296 
2297  for (int Acx = start_cx; Acx <= Acx1; Acx++)
2298  {
2299  ASSERT_(idx_i_in_c >= 0 && idx_i_in_c < int(K));
2300 
2301  double cov_i_c = m_stackedCov(cellIdx, idx_i_in_c);
2302  m_map[idx].kf_mean += yk * sk_1 * cov_i_c;
2303 
2304  // Save window indexes:
2305  cross_covs_c_i[window_idx] = cov_i_c;
2306  window_idxs[window_idx++] = idx;
2307 
2308  idx_i_in_c++;
2309  idx++;
2310  }
2311  }
2312 
2313  // ------------------------------------------------------------
2314  // Update cross-covariances values:
2315  // Process only those cells in the vecinity of the cell (c)
2316  // ------------------------------------------------------------
2317 
2318  // First, we need the cross-covariances between each cell (i) and
2319  // (c) in the window around (c). We have this in "cross_covs_c_i[k]"
2320  // for k=[0,(2K+1)^2-1], and the indexes in "window_idxs[k]".
2321  for (size_t i = 0; i < W21sqr; i++)
2322  {
2323  const int idx_i = window_idxs[i];
2324  if (idx_i < 0) continue; // out of the map
2325 
2326  // Extract the x,y indexes:
2327  int cx_i, cy_i;
2328  idx2cxcy(idx_i, cx_i, cy_i);
2329 
2330  const double cov_c_i = cross_covs_c_i[i];
2331 
2332  for (size_t j = i; j < W21sqr; j++)
2333  {
2334  const int idx_j = window_idxs[j];
2335  if (idx_j < 0) continue; // out of the map
2336 
2337  int cx_j, cy_j;
2338  idx2cxcy(idx_j, cx_j, cy_j);
2339 
2340  // The cells (i,j) may be too far from each other:
2341  const int Ax = cx_j - cx_i;
2342  if (Ax > W)
2343  continue; // Next cell (may be more rows after the current
2344  // one...)
2345 
2346  const int Ay = cy_j - cy_i;
2347  if (Ay > W) break; // We are absolutely sure out of i's window.
2348 
2349  const double cov_c_j = cross_covs_c_i[j];
2350 
2351  int idx_j_in_i;
2352  if (Ay > 0)
2353  idx_j_in_i = Ax + W + (Ay - 1) * W21 + W + 1;
2354  else
2355  idx_j_in_i = Ax; // First row:
2356 
2357  // Kalman update of the cross-covariances:
2358  double& cov_to_change = m_stackedCov(idx_i, idx_j_in_i);
2359  double Delta_cov = cov_c_j * cov_c_i * sk_1;
2360  if (i == j && cov_to_change < Delta_cov)
2362  "Negative variance value appeared! Please increase the "
2363  "size of the window "
2364  "(W).\n(m_insertOptions_common->KF_covSigma=%f)",
2366 
2367  cov_to_change -= Delta_cov;
2368 
2369  } // end for j
2370  } // end for i
2371 
2372  MRPT_LOG_DEBUG_FMT("Done in %.03fms\n", tictac.Tac() * 1000);
2373 
2374  MRPT_END
2375 }
2376 /*---------------------------------------------------------------
2377  recoverMeanAndCov
2378  ---------------------------------------------------------------*/
2380 {
2382  m_hasToRecoverMeanAndCov = false;
2383 
2384  // Just recover the std of each cell:
2385  const size_t N = m_map.size();
2386  for (size_t i = 0; i < N; i++)
2387  m_map_castaway_const()[i].kf_std = sqrt(m_stackedCov(i, 0));
2388 }
2389 
2390 /*---------------------------------------------------------------
2391  getMeanAndCov
2392  ---------------------------------------------------------------*/
2394  CVectorDouble& out_means, CMatrixDouble& out_cov) const
2395 {
2396  const size_t N = BASE::m_map.size();
2397  out_means.resize(N);
2398  for (size_t i = 0; i < N; ++i) out_means[i] = BASE::m_map[i].kf_mean;
2399 
2401  out_cov = m_cov;
2402 }
2403 
2404 /*---------------------------------------------------------------
2405  getMeanAndSTD
2406  ---------------------------------------------------------------*/
2408  CVectorDouble& out_means, CVectorDouble& out_STD) const
2409 {
2410  const size_t N = BASE::m_map.size();
2411  out_means.resize(N);
2412  out_STD.resize(N);
2413 
2414  for (size_t i = 0; i < N; ++i)
2415  {
2416  out_means[i] = BASE::m_map[i].kf_mean;
2417  out_STD[i] = sqrt(m_stackedCov(i, 0));
2418  }
2419 }
2420 
2421 /*---------------------------------------------------------------
2422  setMeanAndSTD
2423  ---------------------------------------------------------------*/
2425  CVectorDouble& in_means, CVectorDouble& in_std)
2426 {
2427  // Assure dimmensions match
2428  const size_t N = BASE::m_map.size();
2429  ASSERT_(N == size_t(in_means.size()));
2430  ASSERT_(N == size_t(in_std.size()));
2431 
2432  m_hasToRecoverMeanAndCov = true;
2433  for (size_t i = 0; i < N; ++i)
2434  {
2435  m_map[i].kf_mean = in_means[i]; // update mean values
2436  m_stackedCov(i, 0) = square(in_std[i]); // update variance values
2437  }
2438  recoverMeanAndCov(); // update STD values from cov matrix
2439 }
2440 
2442 {
2443  return m_mapType;
2444 }
2445 
2447 {
2448  switch (m_mapType)
2449  {
2450  case mrKernelDM:
2451  case mrKernelDMV:
2452  case mrKalmanFilter:
2453  case mrKalmanApproximate:
2454  // Nothing to do, already done in the insert method...
2455  break;
2456 
2457  case mrGMRF_SD:
2458  this->updateMapEstimation_GMRF();
2459  break;
2460  default:
2462  "insertObservation() isn't implemented for selected 'mapType'");
2463  };
2464 }
2465 
2467  const double sensorReading, const mrpt::math::TPoint2D& point,
2468  const bool update_map, const bool time_invariant,
2469  const double reading_stddev)
2470 {
2471  switch (m_mapType)
2472  {
2473  case mrKernelDM:
2474  insertObservation_KernelDM_DMV(sensorReading, point, false);
2475  break;
2476  case mrKernelDMV:
2477  insertObservation_KernelDM_DMV(sensorReading, point, true);
2478  break;
2479  case mrKalmanFilter:
2480  insertObservation_KF(sensorReading, point);
2481  break;
2482  case mrKalmanApproximate:
2483  insertObservation_KF2(sensorReading, point);
2484  break;
2485  case mrGMRF_SD:
2487  sensorReading, point, update_map, time_invariant,
2488  reading_stddev == .0
2490  ->GMRF_lambdaObs // default information
2491  : 1.0 / mrpt::square(reading_stddev));
2492  break;
2493  default:
2495  "insertObservation() isn't implemented for selected 'mapType'");
2496  };
2497 }
2498 
2499 /*---------------------------------------------------------------
2500  insertObservation_GMRF
2501  ---------------------------------------------------------------*/
2503  double normReading, const mrpt::math::TPoint2D& point,
2504  const bool update_map, const bool time_invariant,
2505  const double reading_information)
2506 {
2507  try
2508  {
2509  // Get index of observed cell
2510  const int cellIdx = xy2idx(point.x, point.y);
2511  TRandomFieldCell* cell = cellByPos(point.x, point.y);
2512  ASSERT_(cell != nullptr);
2513 
2514  // Insert observation in the vector of Active Observations
2515  TObservationGMRF new_obs(*this);
2516  new_obs.node_id = cellIdx;
2517  new_obs.obsValue = normReading;
2518  new_obs.Lambda = reading_information;
2519  new_obs.time_invariant = time_invariant;
2520 
2521  m_mrf_factors_activeObs[cellIdx].push_back(new_obs);
2523  *m_mrf_factors_activeObs[cellIdx].rbegin()); // add to graph
2524  }
2525  catch (const std::exception& e)
2526  {
2527  cerr << "Exception while Inserting new Observation: " << e.what()
2528  << endl;
2529  }
2530 
2531  // Solve system and update map estimation
2532  if (update_map) updateMapEstimation_GMRF();
2533 }
2534 
2535 /*---------------------------------------------------------------
2536  updateMapEstimation_GMRF
2537  ---------------------------------------------------------------*/
2539 {
2540  mrpt::math::CVectorDouble x_incr, x_var;
2542  x_incr, m_insertOptions_common->GMRF_skip_variance ? nullptr : &x_var);
2543 
2544  ASSERT_(size_t(m_map.size()) == size_t(x_incr.size()));
2545  ASSERT_(
2547  size_t(m_map.size()) == size_t(x_var.size()));
2548 
2549  // Update Mean-Variance in the base grid class
2550  for (size_t j = 0; j < m_map.size(); j++)
2551  {
2553  ? .0
2554  : std::sqrt(x_var[j]);
2555  m_map[j].gmrf_mean += x_incr[j];
2556 
2560  }
2561 
2562  // Update Information/Strength of Active Observations
2563  //---------------------------------------------------------
2565  {
2566  for (auto& obs : m_mrf_factors_activeObs)
2567  {
2568  for (auto ito = obs.begin(); ito != obs.end();)
2569  {
2570  if (!ito->time_invariant)
2571  {
2572  ++ito;
2573  continue;
2574  }
2575 
2577  if (ito->Lambda < 0)
2578  {
2579  m_gmrf.eraseConstraint(*ito);
2580  ito = obs.erase(ito);
2581  }
2582  else
2583  ++ito;
2584  }
2585  }
2586  }
2587 }
2588 
2590  const mrpt::maps::COccupancyGridMap2D* m_Ocgridmap, size_t cxo_min,
2591  size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo,
2592  const size_t seed_cyo, const size_t objective_cxo,
2593  const size_t objective_cyo)
2594 {
2595  // printf("Checking relation between cells (%i,%i) and (%i,%i)",
2596  // seed_cxo,seed_cyo,objective_cxo,objective_cyo);
2597 
2598  // Ensure delimited region is within the Occupancy map
2599  cxo_min = max(cxo_min, (size_t)0);
2600  cxo_max = min(cxo_max, (size_t)m_Ocgridmap->getSizeX() - 1);
2601  cyo_min = max(cyo_min, (size_t)0);
2602  cyo_max = min(cyo_max, (size_t)m_Ocgridmap->getSizeY() - 1);
2603 
2604  // printf("Under gridlimits cx=(%i,%i) and cy=(%i,%i) \n",
2605  // cxo_min,cxo_max,cyo_min,cyo_max);
2606 
2607  // Check that seed and objective are inside the delimited Occupancy gridmap
2608  if ((seed_cxo < cxo_min) || (seed_cxo >= cxo_max) || (seed_cyo < cyo_min) ||
2609  (seed_cyo >= cyo_max))
2610  {
2611  // cout << "Seed out of bounds (false)" << endl;
2612  return false;
2613  }
2614  if ((objective_cxo < cxo_min) || (objective_cxo >= cxo_max) ||
2615  (objective_cyo < cyo_min) || (objective_cyo >= cyo_max))
2616  {
2617  // cout << "Objective out of bounds (false)" << endl;
2618  return false;
2619  }
2620 
2621  // Check that seed and obj have similar occupancy (0,1)
2622  if ((m_Ocgridmap->getCell(seed_cxo, seed_cyo) < 0.5) !=
2623  (m_Ocgridmap->getCell(objective_cxo, objective_cyo) < 0.5))
2624  {
2625  // cout << "Seed and objective have diff occupation (false)" << endl;
2626  return false;
2627  }
2628 
2629  // Create Matrix for region growing (row,col)
2630  mrpt::math::CMatrixUInt matExp(
2631  cxo_max - cxo_min + 1, cyo_max - cyo_min + 1);
2632  // cout << "Matrix creted with dimension:" << matExp.rows() << " x "
2633  // << matExp.cols() << endl;
2634  // CMatrixF matExp(cxo_max-cxo_min+1, cyo_max-cyo_min+1);
2635  matExp.fill(0);
2636 
2637  // Add seed
2638  matExp(seed_cxo - cxo_min, seed_cyo - cyo_min) = 1;
2639  int seedsOld = 0;
2640  int seedsNew = 1;
2641 
2642  // NOT VERY EFFICIENT!!
2643  while (seedsOld < seedsNew)
2644  {
2645  seedsOld = seedsNew;
2646 
2647  for (int col = 0; col < matExp.cols(); col++)
2648  {
2649  for (int row = 0; row < matExp.rows(); row++)
2650  {
2651  // test if cell needs to be expanded
2652  if (matExp(row, col) == 1)
2653  {
2654  matExp(row, col) = 2; // mark as expanded
2655  // check if neighbourds have similar occupancy (expand)
2656  for (int i = -1; i <= 1; i++)
2657  {
2658  for (int j = -1; j <= 1; j++)
2659  {
2660  // check that neighbour is inside the map
2661  if ((int(row) + j >= 0) &&
2662  (int(row) + j <= int(matExp.rows() - 1)) &&
2663  (int(col) + i >= 0) &&
2664  (int(col) + i <= int(matExp.cols()) - 1))
2665  {
2666  if (!((i == 0 && j == 0) ||
2667  !(matExp(row + j, col + i) == 0)))
2668  {
2669  // check if expand
2670  if ((m_Ocgridmap->getCell(
2671  row + cxo_min, col + cyo_min) <
2672  0.5) ==
2673  (m_Ocgridmap->getCell(
2674  row + j + cxo_min,
2675  col + i + cyo_min) < 0.5))
2676  {
2677  if ((row + j + cxo_min ==
2678  objective_cxo) &&
2679  (col + i + cyo_min ==
2680  objective_cyo))
2681  {
2682  // cout << "Connection Success
2683  // (true)" << endl;
2684  return true; // Objective connected
2685  }
2686  matExp(row + j, col + i) = 1;
2687  seedsNew++;
2688  }
2689  }
2690  }
2691  }
2692  }
2693  }
2694  }
2695  }
2696  }
2697  // if not connection to he objective is found, then return false
2698  // cout << "Connection not found (false)" << endl;
2699  return false;
2700 }
2701 
2703  const mrpt::maps::CMetricMap* otherMap,
2704  const mrpt::poses::CPose3D& otherMapPose,
2705  const TMatchingRatioParams& params) const
2706 {
2707  MRPT_UNUSED_PARAM(otherMap);
2708  MRPT_UNUSED_PARAM(otherMapPose);
2710  return 0;
2711 }
2712 
2713 // ============ TObservationGMRF ===========
2715 {
2716  return m_parent->m_map[this->node_id].gmrf_mean - this->obsValue;
2717 }
2719 {
2720  return this->Lambda;
2721 }
2723 {
2724  dr_dx = 1.0;
2725 }
2726 // ============ TPriorFactorGMRF ===========
2728 {
2729  return m_parent->m_map[this->node_id_i].gmrf_mean -
2730  m_parent->m_map[this->node_id_j].gmrf_mean;
2731 }
2733 {
2734  return this->Lambda;
2735 }
2737  double& dr_dx_i, double& dr_dx_j) const
2738 {
2739  dr_dx_i = +1.0;
2740  dr_dx_j = -1.0;
2741 }
2742 
2744  default;
2746  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) ...
double x
X,Y coordinates.
Definition: TPoint2D.h:23
Parameters for CMetricMap::compute3DMatchingRatio()
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:42
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)...
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].
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
double evaluateResidual() const override
Return the residual/error of this observation.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
#define min(a, b)
void resize(size_t row, size_t col)
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
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.
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...
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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)
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 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...
double gmrf_mean
[GMRF only] The mean value of this cell
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:275
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:586
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
T square(const T x)
Inline function for the square of a number.
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha
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.
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 ...
double kf_mean
[KF-methods only] The mean value of this 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.
void jet2rgb(const float color_index, float &r, float &g, float &b)
Computes the RGB color components (range [0,1]) for the corresponding color index in the range [0...
Definition: color_maps.cpp:142
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)...
const GLubyte * c
Definition: glext.h:6406
GLint GLvoid * img
Definition: glext.h:3769
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.
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const TRandomFieldCell &defaultValueNewCells, double additionalMarginMeters=2.0)
Changes the size of the grid, maintaining previous contents.
Definition: CDynamicGrid.h:117
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:830
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.
GLsizei const GLchar ** string
Definition: glext.h:4116
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:410
void setCellsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
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.
auto dir
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
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:84
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLenum GLenum GLvoid * row
Definition: glext.h:3580
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.
GLenum GLint GLint y
Definition: glext.h:3542
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.
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.
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:336
double evaluateResidual() const override
Return the residual/error of this observation.
double Lambda
"Information" of the observation (=inverse of the variance)
void idx2cxcy(const int &idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.
Definition: CDynamicGrid.h:271
double dm_mean
[Kernel-methods only] The cumulative weighted readings of this cell
void resize(std::size_t N, bool zeroNewElements=false)
GLenum GLint x
Definition: glext.h:3542
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.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
Lightweight 2D point.
Definition: TPoint2D.h:31
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
!
Definition: CMetricMap.h:107
GLenum const GLfloat * params
Definition: glext.h:3538
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 kf_std
[KF-methods only] The standard deviation value of this cell
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:147
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019