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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST