45 double y_max,
double resolution)
64 const double x_min,
const double x_max,
const double y_min,
65 const double y_max,
const double resolution,
69 x_min, x_max, y_min, y_max, resolution, fill_value);
76 m_gmrf_connectivity = new_connectivity_descriptor;
85 m_insertOptions_common =
86 getCommonInsertOptions();
88 m_average_normreadings_mean = 0;
89 m_average_normreadings_var = 0;
90 m_average_normreadings_count = 0;
106 "[clear] Setting covariance matrix to %ux%u\n",
107 (
unsigned int)(m_size_y * m_size_x),
108 (
unsigned int)(m_size_y * m_size_x));
111 m_insertOptions_common->KF_defaultCellMeanValue,
112 m_insertOptions_common->KF_initialCellStd
118 m_cov.setSize(m_size_y * m_size_x, m_size_y * m_size_x);
121 const double KF_covSigma2 =
122 square(m_insertOptions_common->KF_covSigma);
123 const double res2 =
square(m_resolution);
124 const double std0sqr =
125 square(m_insertOptions_common->KF_initialCellStd);
127 for (
int i = 0; i < m_cov.rows(); i++)
129 int cx1 = (i % m_size_x);
130 int cy1 = (i / m_size_x);
132 for (
int j = i; j < m_cov.cols(); j++)
134 int cx2 = (j % m_size_x);
135 int cy2 = (j / m_size_x);
139 m_cov(i, j) = std0sqr;
145 (res2 *
static_cast<double>(
149 m_cov(j, i) = m_cov(i, j);
158 case mrKalmanApproximate:
160 m_hasToRecoverMeanAndCov =
true;
166 "[CRandomFieldGridMap2D::clear] Resetting compressed cov. "
167 "matrix and cells\n");
169 m_insertOptions_common->KF_defaultCellMeanValue,
170 m_insertOptions_common->KF_initialCellStd
177 const signed W = m_insertOptions_common->KF_W_size;
178 const size_t N = m_map.size();
179 const size_t K = 2 * W * (W + 1) + 1;
181 const double KF_covSigma2 =
182 square(m_insertOptions_common->KF_covSigma);
183 const double std0sqr =
184 square(m_insertOptions_common->KF_initialCellStd);
185 const double res2 =
square(m_resolution);
187 m_stackedCov.setSize(N, K);
192 const double* ptr_first_row = &m_stackedCov(0, 0);
194 for (
size_t i = 0; i < N; i++)
196 double* ptr = &m_stackedCov(i, 0);
205 for (Acx = 1; Acx <= W; Acx++)
208 (res2 *
static_cast<double>(
213 for (Acy = 1; Acy <= W; Acy++)
214 for (Acx = -W; Acx <= W; Acx++)
218 (res2 *
static_cast<double>(
225 memcpy(ptr, ptr_first_row,
sizeof(
double) * K);
230 "[clear] %ux%u cells done in %.03fms\n",
unsigned(m_size_x),
231 unsigned(m_size_y), 1000 * tictac.
Tac());
241 "[clear] Generating Prior based on 'Squared Differences'\n");
243 "[clear] Initial map dimension: %u cells, x=(%.2f,%.2f) "
245 static_cast<unsigned int>(m_map.size()), getXMin(), getXMax(),
246 getYMin(), getYMax());
253 float res_coef = 1.f;
255 if (this->m_insertOptions_common->GMRF_use_occupancy_information)
257 if (!m_insertOptions_common->GMRF_simplemap_file.empty())
261 this->m_insertOptions_common->GMRF_simplemap_file);
268 else if (!m_insertOptions_common->GMRF_gridmap_image_file
273 this->m_insertOptions_common->GMRF_gridmap_image_file,
274 this->m_insertOptions_common->GMRF_gridmap_image_res,
276 this->m_insertOptions_common->GMRF_gridmap_image_cx,
277 this->m_insertOptions_common
278 ->GMRF_gridmap_image_cy));
281 this->getResolution() /
282 this->m_insertOptions_common->GMRF_gridmap_image_res;
287 "Neither `simplemap_file` nor `gridmap_image_file` "
288 "found in config/mission file. Quitting.");
293 "Resizing m_map to match Occupancy Gridmap dimensions");
300 "Occupancy Gridmap dimensions: x=(%.2f,%.2f)m "
305 "Occupancy Gridmap dimensions: %u cells, cx=%i cy=%i\n\n",
306 static_cast<unsigned int>(
310 "New map dimensions: %u cells, x=(%.2f,%.2f) "
312 static_cast<unsigned int>(m_map.size()), getXMin(),
313 getXMax(), getYMin(), getYMax());
315 "New map dimensions: %u cells, cx=%u cy=%u\n",
316 static_cast<unsigned int>(m_map.size()),
317 static_cast<unsigned int>(getSizeX()),
318 static_cast<unsigned int>(getSizeY()));
321 "./obstacle_map_from_MRPT_for_GMRF.png");
325 const size_t nodeCount = m_map.size();
328 const size_t nPriorFactors =
329 (this->getSizeX() - 1) * this->getSizeY() +
335 "[clear] Generating "
337 <<
" prior factors for a map size of N=" << nodeCount << endl);
340 m_gmrf.initialize(nodeCount);
342 m_mrf_factors_activeObs.clear();
343 m_mrf_factors_activeObs.resize(
346 m_mrf_factors_priors.clear();
351 if (!m_gmrf_connectivity &&
352 this->m_insertOptions_common->GMRF_use_occupancy_information)
356 "MRF Map Dimmensions: %u x %u cells \n",
357 static_cast<unsigned int>(m_size_x),
358 static_cast<unsigned int>(m_size_y));
360 "Occupancy map Dimmensions: %i x %i cells \n",
369 size_t cxoj_min, cxoj_max, cyoj_min, cyoj_max, seed_cxo,
371 size_t cxoi_min, cxoi_max, cyoi_min, cyoi_max, objective_cxo,
373 size_t cxo_min, cxo_max, cyo_min,
377 std::multimap<size_t, size_t>
378 cell_interconnections;
382 for (
size_t j = 0; j < nodeCount;
386 cxoj_min = floor(cx * res_coef);
387 cxoj_max = cxoj_min + ceil(res_coef - 1);
388 cyoj_min = floor(cy * res_coef);
389 cyoj_max = cyoj_min + ceil(res_coef - 1);
391 seed_cxo = cxoj_min + ceil(res_coef / 2 - 1);
392 seed_cyo = cyoj_min + ceil(res_coef / 2 - 1);
396 if (m_Ocgridmap.
getCell(seed_cxo, seed_cyo) < 0.5)
404 m_mrf_factors_activeObs[j].push_back(new_obs);
405 m_gmrf.addConstraint(*m_mrf_factors_activeObs[j]
412 for (
int neighbor = 0; neighbor < 2; neighbor++)
418 if (cx >= (m_size_x - 1))
continue;
423 else if (neighbor == 1)
425 if (cy >= (m_size_y - 1))
continue;
431 throw std::runtime_error(
"Shouldn't reach here!");
434 cxoi_min = floor(cxi * res_coef);
435 cxoi_max = cxoi_min + ceil(res_coef - 1);
436 cyoi_min = floor(cyi * res_coef);
437 cyoi_max = cyoi_min + ceil(res_coef - 1);
439 objective_cxo = cxoi_min + ceil(res_coef / 2 - 1);
440 objective_cyo = cyoi_min + ceil(res_coef / 2 - 1);
443 cxo_min = min(cxoj_min, cxoi_min);
444 cxo_max = max(cxoj_max, cxoi_max);
445 cyo_min = min(cyoj_min, cyoi_min);
446 cyo_max = max(cyoj_max, cyoi_max);
450 if (exist_relation_between2cells(
451 &m_Ocgridmap, cxo_min, cxo_max, cyo_min,
452 cyo_max, seed_cxo, seed_cyo, objective_cxo,
459 m_insertOptions_common->GMRF_lambdaPrior;
461 m_mrf_factors_priors.push_back(new_prior);
462 m_gmrf.addConstraint(
463 *m_mrf_factors_priors
467 cell_interconnections.insert(
468 std::pair<size_t, size_t>(j, i));
469 cell_interconnections.insert(
470 std::pair<size_t, size_t>(i, j));
476 if (++cx >= m_size_x)
486 m_gmrf_connectivity.get();
488 if (custom_connectivity !=
nullptr)
490 "[CRandomFieldGridMap2D::clear] Initiating prior "
491 "(using user-supplied connectivity pattern)");
494 "[CRandomFieldGridMap2D::clear] Initiating prior "
495 "(fully connected)");
501 size_t cx = 0, cy = 0;
502 for (
size_t j = 0; j < nodeCount; j++)
508 const size_t c_idx_to_check[2] = {cx, cy};
509 const size_t c_idx_to_check_limits[2] = {m_size_x - 1,
511 const size_t c_neighbor_idx_incr[2] = {1, m_size_x};
515 if (c_idx_to_check[
dir] >= c_idx_to_check_limits[
dir])
518 const size_t i = j + c_neighbor_idx_incr[
dir];
521 double edge_lamdba = .0;
522 if (custom_connectivity !=
nullptr)
524 const bool is_connected =
526 this, cx, cy, cx + (
dir == 0 ? 1 : 0),
527 cy + (
dir == 1 ? 1 : 0), edge_lamdba);
528 if (!is_connected)
continue;
533 m_insertOptions_common->GMRF_lambdaPrior;
538 new_prior.
Lambda = edge_lamdba;
540 m_mrf_factors_priors.push_back(new_prior);
541 m_gmrf.addConstraint(
542 *m_mrf_factors_priors.rbegin());
546 if (++cx >= m_size_x)
555 "[clear] Prior built in " << tictac.
Tac() <<
" s ----------");
557 if (m_rfgm_run_update_upon_clear)
560 updateMapEstimation_GMRF();
566 cerr <<
"MAP TYPE NOT RECOGNIZED... QUITTING" << endl;
592 point.
x - m_insertOptions_common->cutoffRadius * 2,
593 point.
x + m_insertOptions_common->cutoffRadius * 2,
594 point.
y - m_insertOptions_common->cutoffRadius * 2,
595 point.
y + m_insertOptions_common->cutoffRadius * 2, defCell);
599 int Ac_cutoff =
round(m_insertOptions_common->cutoffRadius / m_resolution);
600 unsigned Ac_all = 1 + 2 * Ac_cutoff;
601 double minWinValueAtCutOff = exp(-
square(
602 m_insertOptions_common->cutoffRadius / m_insertOptions_common->sigma));
604 if (m_DM_lastCutOff != m_insertOptions_common->cutoffRadius ||
605 m_DM_gaussWindow.size() !=
square(Ac_all))
608 "[CRandomFieldGridMap2D::insertObservation_KernelDM_DMV] "
609 "Precomputing window %ux%u\n",
613 double std = m_insertOptions_common->sigma;
616 m_DM_gaussWindow.resize(Ac_all * Ac_all);
617 m_DM_lastCutOff = m_insertOptions_common->cutoffRadius;
621 auto it = m_DM_gaussWindow.begin();
622 for (
unsigned cx = 0; cx < Ac_all; cx++)
624 for (
unsigned cy = 0; cy < Ac_all; cy++)
626 dist = m_resolution * sqrt(
static_cast<double>(
627 square(Ac_cutoff + 1 - cx) +
628 square(Ac_cutoff + 1 - cy)));
629 *(it++) = std::exp(-
square(dist / std));
634 "[CRandomFieldGridMap2D::insertObservation_KernelDM_DMV] Done!");
639 const int sensor_cx = x2idx(point.
x);
640 const int sensor_cy = y2idx(point.
y);
642 auto windowIt = m_DM_gaussWindow.begin();
644 for (
int Acx = -Ac_cutoff; Acx <= Ac_cutoff; Acx++)
646 for (
int Acy = -Ac_cutoff; Acy <= Ac_cutoff; ++Acy, ++windowIt)
648 const double windowValue = *windowIt;
650 if (windowValue > minWinValueAtCutOff)
652 cell = cellByIndex(sensor_cx + Acx, sensor_cy + Acy);
655 cell->
dm_mean() += windowValue * normReading;
658 const double cell_var =
659 square(normReading - computeMeanCellValue_DM_DMV(cell));
673 : cutoffRadius(sigma * 3.0),
675 GMRF_simplemap_file(
""),
676 GMRF_gridmap_image_file(
""),
678 GMRF_saturate_min(-std::numeric_limits<double>::max()),
679 GMRF_saturate_max(std::numeric_limits<double>::max())
691 "sigma = %f\n", sigma);
693 "cutoffRadius = %f\n", cutoffRadius);
695 "R_min = %f\n", R_min);
697 "R_max = %f\n", R_max);
699 "dm_sigma_omega = %f\n", dm_sigma_omega);
702 "KF_covSigma = %f\n", KF_covSigma);
704 "KF_initialCellStd = %f\n", KF_initialCellStd);
706 "KF_observationModelNoise = %f\n",
707 KF_observationModelNoise);
709 "KF_defaultCellMeanValue = %f\n",
710 KF_defaultCellMeanValue);
712 "KF_W_size = %u\n", (
unsigned)KF_W_size);
715 "GMRF_lambdaPrior = %f\n", GMRF_lambdaPrior);
717 "GMRF_lambdaObs = %f\n", GMRF_lambdaObs);
719 "GMRF_lambdaObsLoss = %f\n", GMRF_lambdaObs);
722 "GMRF_use_occupancy_information = %s\n",
723 GMRF_use_occupancy_information ?
"YES" :
"NO");
725 "GMRF_simplemap_file = %s\n",
726 GMRF_simplemap_file.c_str());
728 "GMRF_gridmap_image_file = %s\n",
729 GMRF_gridmap_image_file.c_str());
731 "GMRF_gridmap_image_res = %f\n",
732 GMRF_gridmap_image_res);
734 "GMRF_gridmap_image_cx = %u\n",
735 static_cast<unsigned int>(GMRF_gridmap_image_cx));
737 "GMRF_gridmap_image_cy = %u\n",
738 static_cast<unsigned int>(GMRF_gridmap_image_cy));
747 const std::string& section)
749 sigma =
iniFile.read_float(section.c_str(),
"sigma", sigma);
751 iniFile.read_float(section.c_str(),
"cutoffRadius", cutoffRadius);
752 R_min =
iniFile.read_float(section.c_str(),
"R_min", R_min);
753 R_max =
iniFile.read_float(section.c_str(),
"R_max", R_max);
757 iniFile.read_float(section.c_str(),
"KF_covSigma", KF_covSigma);
758 KF_initialCellStd =
iniFile.read_float(
759 section.c_str(),
"KF_initialCellStd", KF_initialCellStd);
760 KF_observationModelNoise =
iniFile.read_float(
761 section.c_str(),
"KF_observationModelNoise", KF_observationModelNoise);
762 KF_defaultCellMeanValue =
iniFile.read_float(
763 section.c_str(),
"KF_defaultCellMeanValue", KF_defaultCellMeanValue);
766 GMRF_lambdaPrior =
iniFile.read_float(
767 section.c_str(),
"GMRF_lambdaPrior", GMRF_lambdaPrior);
769 iniFile.read_float(section.c_str(),
"GMRF_lambdaObs", GMRF_lambdaObs);
770 GMRF_lambdaObsLoss =
iniFile.read_float(
771 section.c_str(),
"GMRF_lambdaObsLoss", GMRF_lambdaObsLoss);
773 GMRF_use_occupancy_information =
iniFile.read_bool(
774 section.c_str(),
"GMRF_use_occupancy_information",
false,
false);
775 GMRF_simplemap_file =
776 iniFile.read_string(section.c_str(),
"simplemap_file",
"",
false);
777 GMRF_gridmap_image_file =
778 iniFile.read_string(section.c_str(),
"gridmap_image_file",
"",
false);
779 GMRF_gridmap_image_res =
780 iniFile.read_float(section.c_str(),
"gridmap_image_res", 0.01f,
false);
781 GMRF_gridmap_image_cx =
782 iniFile.read_int(section.c_str(),
"gridmap_image_cx", 0,
false);
783 GMRF_gridmap_image_cy =
784 iniFile.read_int(section.c_str(),
"gridmap_image_cy", 0,
false);
796 img.saveToFile(filName);
809 for (
unsigned int y = 0; y <
m_size_y; y++)
811 for (
unsigned int x = 0; x <
m_size_x; x++)
861 double new_x_min,
double new_x_max,
double new_y_min,
double new_y_max,
862 const TRandomFieldCell& defaultValueNewCells,
double additionalMarginMeters)
873 new_x_min, new_x_max, new_y_min, new_y_max, defaultValueNewCells,
874 additionalMarginMeters);
893 "[CRandomFieldGridMap2D::resize] Resizing from %ux%u to %ux%u "
895 static_cast<unsigned>(old_sizeX),
896 static_cast<unsigned>(old_sizeY),
910 for (i = 0; i < N; i++)
915 bool C1_isFromOldMap =
916 Acx_left <= cx1 && cx1 < (Acx_left + old_sizeX) &&
917 Acy_bottom <= cy1 && cy1 < (Acy_bottom + old_sizeY);
921 for (j = i; j < N; j++)
926 bool C2_isFromOldMap =
927 Acx_left <= cx2 && cx2 < (Acx_left + old_sizeX) &&
928 Acy_bottom <= cy2 && cy2 < (Acy_bottom + old_sizeY);
931 if (C1_isFromOldMap && C2_isFromOldMap)
934 unsigned int idx_c1 =
936 old_sizeX * (cy1 - Acy_bottom));
937 unsigned int idx_c2 =
939 old_sizeX * (cy2 - Acy_bottom));
945 ASSERT_((cx1 - Acx_left) < old_sizeX);
946 ASSERT_((cy1 - Acy_bottom) < old_sizeY);
950 ASSERT_((cx2 - Acx_left) < old_sizeX);
951 ASSERT_((cy2 - Acy_bottom) < old_sizeY);
953 ASSERT_(idx_c1 < old_sizeX * old_sizeY);
954 ASSERT_(idx_c2 < old_sizeX * old_sizeY);
957 printf(
"cx1=%u\n",
static_cast<unsigned>(cx1));
958 printf(
"cy1=%u\n",
static_cast<unsigned>(cy1));
959 printf(
"cx2=%u\n",
static_cast<unsigned>(cx2));
960 printf(
"cy2=%u\n",
static_cast<unsigned>(cy2));
963 static_cast<unsigned>(Acx_left));
966 static_cast<unsigned>(Acy_bottom));
969 static_cast<unsigned>(idx_c1));
972 static_cast<unsigned>(idx_c2)););
974 m_cov(i, j) = oldCov(idx_c1, idx_c2);
977 if (i == j)
ASSERT_(idx_c1 == idx_c2);
979 if (i == j &&
m_cov(i, i) < 0)
982 "\ni=%u \nj=%i \ncx1,cy1 = %u,%u \n "
983 "cx2,cy2=%u,%u \nidx_c1=%u \nidx_c2=%u "
984 "\nAcx_left=%u \nAcy_bottom=%u "
986 static_cast<unsigned>(i),
987 static_cast<unsigned>(j),
988 static_cast<unsigned>(cx1),
989 static_cast<unsigned>(cy1),
990 static_cast<unsigned>(cx2),
991 static_cast<unsigned>(cy2),
992 static_cast<unsigned>(idx_c1),
993 static_cast<unsigned>(idx_c2),
994 static_cast<unsigned>(Acx_left),
995 static_cast<unsigned>(Acy_bottom),
996 static_cast<unsigned>(old_sizeX));
1009 for (i = 0; i < N; i++)
1014 bool C1_isFromOldMap =
1015 Acx_left <= cx1 && cx1 < (Acx_left + old_sizeX) &&
1016 Acy_bottom <= cy1 && cy1 < (Acy_bottom + old_sizeY);
1017 for (j = i; j < N; j++)
1022 bool C2_isFromOldMap =
1023 Acx_left <= cx2 && cx2 < (Acx_left + old_sizeX) &&
1024 Acy_bottom <= cy2 && cy2 < (Acy_bottom + old_sizeY);
1029 if (!C1_isFromOldMap || !C2_isFromOldMap)
1040 sqrt(
static_cast<double>(
1047 "c(i,i)=%e c(j,j)=%e\n",
m_cov(i, i),
1068 printf(
"[CRandomFieldGridMap2D::resize] Done\n");
1083 "[resize] Resizing from %ux%u to %ux%u (%u cells)\n",
1084 static_cast<unsigned>(old_sizeX),
1085 static_cast<unsigned>(old_sizeY),
1092 const size_t N =
m_map.size();
1093 const size_t K = 2 * W * (W + 1) + 1;
1106 const double std0sqr =
1108 double* ptr = &template_row[0];
1110 const double KF_covSigma2 =
1118 for (Acx = 1; Acx <= W; Acx++)
1120 std0sqr * exp(-0.5 *
1121 (res2 *
static_cast<double>(
1126 for (Acy = 1; Acy <= W; Acy++)
1127 for (Acx = -W; Acx <= W; Acx++)
1130 (res2 *
static_cast<double>(
1138 for (
size_t i = N - 1; i < N; i--)
1143 const int old_idx_of_i =
1144 (cx - Acx_left) + (cy - Acy_bottom) * old_sizeX;
1154 memcpy(new_row, &template_row[0],
sizeof(
double) * K);
1161 if (old_idx_of_i !=
int(i))
1163 const double* ptr_old = &
m_stackedCov(old_idx_of_i, 0);
1165 memcpy(ptr_new, ptr_old,
sizeof(
double) * K);
1189 resize(point.
x - 1, point.
x + 1, point.
y - 1, point.
y + 1, defCell);
1203 int cellIdx =
xy2idx(point.
x, point.
y);
1208 double yk = normReading - cell->
kf_mean();
1210 m_cov(cellIdx, cellIdx) +
1213 ->KF_observationModelNoise);
1214 double sk_1 = 1.0 / sk;
1217 std::vector<TRandomFieldCell>::iterator it;
1220 MRPT_LOG_DEBUG(
"[insertObservation_KF] Updating mean values...");
1225 for (i = 0, it =
m_map.begin(); it !=
m_map.end(); ++it, ++i)
1227 it->kf_mean() += yk * sk_1 *
m_cov(i, cellIdx);
1235 MRPT_LOG_DEBUG(
"[insertObservation_KF] Updating covariance matrix...");
1239 auto* oldCov = (
double*) malloc(
sizeof(
double) * N * N);
1240 double* oldCov_ptr = oldCov;
1241 for (i = 0; i < N; i++)
1243 memcpy(oldCov_ptr, &
m_cov(i, 0),
sizeof(
double) * N);
1248 "Copy matrix %ux%u: %.06fms\n", (
unsigned)
m_cov.
rows(),
1254 const double* oldCov_row_c = oldCov + cellIdx * N;
1255 for (i = 0; i < N; i++)
1257 const double oldCov_i_c = oldCov[i * N + cellIdx];
1258 const double sk_1_oldCov_i_c = sk_1 * oldCov_i_c;
1260 const double* oldCov_row_i = oldCov + i * N;
1261 for (j = i; j < N; j++)
1264 oldCov_row_i[j] - sk_1_oldCov_i_c * oldCov_row_c[j];
1267 m_cov(i, j) = new_cov_ij;
1268 m_cov(j, i) = new_cov_ij;
1273 if (
m_cov(i, i) < 0)
1276 "Wrong insertion in KF! m_cov(%u,%u) = %.5f",
1277 static_cast<unsigned int>(i),
1278 static_cast<unsigned int>(i),
m_cov(i, i));
1282 m_map[i].kf_std() = sqrt(new_cov_ij);
1299 const std::string& filNamePrefix)
const
1305 fil = filNamePrefix + std::string(
"_mean.png");
1319 "% Grid limits: [x_min x_max y_min y_max]\n");
1330 for (
size_t y = 0; y <
m_size_y; y++)
1331 for (
size_t x = 0; x <
m_size_x; x++)
1344 filNamePrefix + std::string(
"_var.txt"),
1347 filNamePrefix + std::string(
"_confidence.txt"),
1362 for (
size_t i = 0; i <
m_size_y; i++)
1363 for (
size_t j = 0; j <
m_size_x; j++)
1372 filNamePrefix + std::string(
"_cells_std.txt"),
1378 filNamePrefix + std::string(
"_mean_compressed_cov.txt"),
1385 filNamePrefix + std::string(
"_mean_cov.txt"));
1392 filNamePrefix + std::string(
"_cells_std.png"),
1408 for (
size_t i = 0; i <
m_size_y; ++i)
1410 for (
size_t j = 0; j <
m_size_x; ++j, ++idx)
1415 XYZ(idx, 0) =
idx2x(j);
1416 XYZ(idx, 1) =
idx2y(i);
1425 filNamePrefix + std::string(
"_cells_std.txt"),
1428 filNamePrefix + std::string(
"_xyz_and_std.txt"),
1430 "% Columns: GRID_X GRID_Y ESTIMATED_Z "
1431 "STD_DEV_OF_ESTIMATED_Z \n");
1444 const std::string& filName)
const
1448 const double std_times = 3;
1456 FILE* f =
os::fopen(filName.c_str(),
"wt");
1460 f,
"%%-------------------------------------------------------\n");
1461 os::fprintf(f,
"%% File automatically generated using the MRPT method:\n");
1462 os::fprintf(f,
"%%'CRandomFieldGridMap2D::saveAsMatlab3DGraph'\n");
1466 f,
"%% Jose Luis Blanco Claraco, University of Malaga @ 2006-2007\n");
1469 f,
"%%-------------------------------------------------------\n\n");
1471 unsigned int cx, cy;
1472 vector<double> xs, ys;
1556 os::fprintf(f,
"set(S1,'FaceAlpha',0.9,'EdgeAlpha',0.9);\n");
1557 os::fprintf(f,
"set(S2,'FaceAlpha',0.4,'EdgeAlpha',0.4);\n");
1558 os::fprintf(f,
"set(S3,'FaceAlpha',0.2,'EdgeAlpha',0.2);\n");
1561 f,
"set(gca,'PlotBoxAspectRatio',[%f %f %f]);\n",
m_x_max -
m_x_min,
1569 "\nfigure; imagesc(xs,ys,z_mean);axis equal;title('Mean "
1570 "value');colorbar;");
1573 "\nfigure; imagesc(xs,ys,(z_upper-z_mean)/%f);axis equal;title('Std "
1574 "dev of estimated value');colorbar;",
1609 unsigned int cx, cy;
1610 vector<double> xs, ys;
1629 auto obj_m = std::make_shared<opengl::CSetOfTriangles>();
1630 auto obj_v = std::make_shared<opengl::CSetOfTriangles>();
1634 double maxVal_m = 0, minVal_m = 1, AMaxMin_m, maxVal_v = 0,
1635 minVal_v = 1, AMaxMin_v;
1645 minVal_m = min(minVal_m, c);
1646 maxVal_m = max(maxVal_m, c);
1649 minVal_v = min(minVal_v, v);
1650 maxVal_v = max(maxVal_v, v);
1654 AMaxMin_m = maxVal_m - minVal_m;
1655 if (AMaxMin_m == 0) AMaxMin_m = 1;
1656 AMaxMin_v = maxVal_v - minVal_v;
1657 if (AMaxMin_v == 0) AMaxMin_v = 1;
1663 triag.
a(0) = triag.
a(1) = triag.
a(2) = 0.75f;
1672 ASSERT_(cell_x_1y !=
nullptr);
1674 ASSERT_(cell_xy_1 !=
nullptr);
1677 ASSERT_(cell_x_1y_1 !=
nullptr);
1698 double col_xy = c_xy;
1699 double col_x_1y = c_x_1y;
1700 double col_xy_1 = c_xy_1;
1701 double col_x_1y_1 = c_x_1y_1;
1704 triag.
x(0) = xs[cx];
1705 triag.
y(0) = ys[cy];
1707 triag.
x(1) = xs[cx];
1708 triag.
y(1) = ys[cy - 1];
1709 triag.
z(1) = c_xy_1;
1710 triag.
x(2) = xs[cx - 1];
1711 triag.
y(2) = ys[cy - 1];
1712 triag.
z(2) = c_x_1y_1;
1718 obj_m->insertTriangle(triag);
1721 triag.
x(0) = xs[cx];
1722 triag.
y(0) = ys[cy];
1724 triag.
x(1) = xs[cx - 1];
1725 triag.
y(1) = ys[cy - 1];
1726 triag.
z(1) = c_x_1y_1;
1727 triag.
x(2) = xs[cx - 1];
1728 triag.
y(2) = ys[cy];
1729 triag.
z(2) = c_x_1y;
1733 obj_m->insertTriangle(triag);
1749 col_x_1y = v_x_1y / 10;
1751 col_xy_1 = v_xy_1 / 10;
1753 col_x_1y_1 = v_x_1y_1 / 10;
1757 triag.
x(0) = xs[cx];
1758 triag.
y(0) = ys[cy];
1759 triag.
z(0) = c_xy + v_xy;
1760 triag.
x(1) = xs[cx];
1761 triag.
y(1) = ys[cy - 1];
1762 triag.
z(1) = c_xy_1 + v_xy_1;
1763 triag.
x(2) = xs[cx - 1];
1764 triag.
y(2) = ys[cy - 1];
1765 triag.
z(2) = c_x_1y_1 + v_x_1y_1;
1771 obj_v->insertTriangle(triag);
1774 triag.
x(0) = xs[cx];
1775 triag.
y(0) = ys[cy];
1776 triag.
z(0) = c_xy + v_xy;
1777 triag.
x(1) = xs[cx - 1];
1778 triag.
y(1) = ys[cy - 1];
1779 triag.
z(1) = c_x_1y_1 + v_x_1y_1;
1780 triag.
x(2) = xs[cx - 1];
1781 triag.
y(2) = ys[cy];
1782 triag.
z(2) = c_x_1y + v_x_1y;
1788 obj_v->insertTriangle(triag);
1792 meanObj->insert(obj_m);
1793 varObj->insert(obj_v);
1802 auto obj_m = std::make_shared<opengl::CSetOfTriangles>();
1803 auto obj_v = std::make_shared<opengl::CSetOfTriangles>();
1807 double maxVal_m = 0, minVal_m = 1, AMaxMin_m, maxVal_v = 0,
1808 minVal_v = 1, AMaxMin_v;
1818 minVal_m = min(minVal_m, c);
1819 maxVal_m = max(maxVal_m, c);
1822 minVal_v = min(minVal_v, v);
1823 maxVal_v = max(maxVal_v, v);
1827 AMaxMin_m = maxVal_m - minVal_m;
1828 if (AMaxMin_m == 0) AMaxMin_m = 1;
1829 AMaxMin_v = maxVal_v - minVal_v;
1830 if (AMaxMin_v == 0) AMaxMin_v = 1;
1835 triag.
a(0) = triag.
a(1) = triag.
a(2) =
1845 ASSERT_(cell_x_1y !=
nullptr);
1847 ASSERT_(cell_xy_1 !=
nullptr);
1850 ASSERT_(cell_x_1y_1 !=
nullptr);
1871 double col_xy = c_xy;
1872 double col_x_1y = c_x_1y;
1873 double col_xy_1 = c_xy_1;
1874 double col_x_1y_1 = c_x_1y_1;
1877 triag.
x(0) = xs[cx];
1878 triag.
y(0) = ys[cy];
1880 triag.
x(1) = xs[cx];
1881 triag.
y(1) = ys[cy - 1];
1882 triag.
z(1) = c_xy_1;
1883 triag.
x(2) = xs[cx - 1];
1884 triag.
y(2) = ys[cy - 1];
1885 triag.
z(2) = c_x_1y_1;
1891 obj_m->insertTriangle(triag);
1894 triag.
x(0) = xs[cx];
1895 triag.
y(0) = ys[cy];
1897 triag.
x(1) = xs[cx - 1];
1898 triag.
y(1) = ys[cy - 1];
1899 triag.
z(1) = c_x_1y_1;
1900 triag.
x(2) = xs[cx - 1];
1901 triag.
y(2) = ys[cy];
1902 triag.
z(2) = c_x_1y;
1908 obj_m->insertTriangle(triag);
1914 double v_x_1y = min(
1916 double v_xy_1 = min(
1918 double v_x_1y_1 = min(
1924 col_x_1y_1 = v_x_1y_1;
1927 triag.
x(0) = xs[cx];
1928 triag.
y(0) = ys[cy];
1930 triag.
x(1) = xs[cx];
1931 triag.
y(1) = ys[cy - 1];
1932 triag.
z(1) = v_xy_1;
1933 triag.
x(2) = xs[cx - 1];
1934 triag.
y(2) = ys[cy - 1];
1935 triag.
z(2) = v_x_1y_1;
1941 obj_v->insertTriangle(triag);
1944 triag.
x(0) = xs[cx];
1945 triag.
y(0) = ys[cy];
1947 triag.
x(1) = xs[cx - 1];
1948 triag.
y(1) = ys[cy - 1];
1949 triag.
z(1) = v_x_1y_1;
1950 triag.
x(2) = xs[cx - 1];
1951 triag.
y(2) = ys[cy];
1952 triag.
z(2) = v_x_1y;
1958 obj_v->insertTriangle(triag);
1962 meanObj->insert(obj_m);
1963 varObj->insert(obj_v);
1988 const double alpha =
1991 const double r_val =
2003 const double alpha =
2006 const double r_val =
2022 const double x,
const double y,
double& out_predict_response,
2023 double& out_predict_response_variance,
bool do_sensor_normalization,
2028 vector<TInterpQuery> queries;
2029 switch (interp_method)
2033 queries[0].cx =
x2idx(x);
2034 queries[0].cy =
y2idx(y);
2035 queries[0].coef = 1.0;
2046 queries[0].cx =
x2idx(x);
2047 queries[0].cy =
y2idx(y);
2048 queries[0].coef = 1.0;
2067 queries[0].coef = K_1 * (
idx2x(queries[3].cx) - x) *
2068 (
idx2y(queries[3].cy) - y);
2069 queries[1].coef = K_1 * (
idx2x(queries[3].cx) - x) *
2070 (y -
idx2y(queries[0].cy));
2071 queries[2].coef = K_1 * (x -
idx2x(queries[0].cx)) *
2072 (
idx2y(queries[3].cy) - y);
2073 queries[3].coef = K_1 * (x -
idx2x(queries[0].cx)) *
2074 (y -
idx2y(queries[0].cy));
2082 for (
auto& q : queries)
2149 out_predict_response = 0;
2150 out_predict_response_variance = 0;
2151 for (
auto& querie : queries)
2153 out_predict_response += querie.val * querie.coef;
2154 out_predict_response_variance += querie.var * querie.coef;
2158 if (do_sensor_normalization)
2159 out_predict_response =
2161 out_predict_response *
2176 "Inserting KF2: (" << normReading <<
") at Position" << point << endl);
2179 const size_t K = 2 * W * (W + 1) + 1;
2180 const size_t W21 = 2 * W + 1;
2181 const size_t W21sqr = W21 * W21;
2197 point.
x - Aspace, point.
x + Aspace, point.
y - Aspace, point.
y + Aspace,
2203 const size_t N =
m_map.size();
2220 const int cellIdx =
xy2idx(point.
x, point.
y);
2225 double yk = normReading - cell->
kf_mean();
2231 double sk_1 = 1.0 / sk;
2234 MRPT_LOG_DEBUG(
"[insertObservation_KF2] Updating mean values...");
2243 const int cx_c =
x2idx(point.
x);
2244 const int cy_c =
y2idx(point.
y);
2246 const int Acx0 = max(-W, -cx_c);
2247 const int Acy0 = max(-W, -cy_c);
2248 const int Acx1 = min(W,
int(
m_size_x) - 1 - cx_c);
2249 const int Acy1 = min(W,
int(
m_size_y) - 1 - cy_c);
2256 std::vector<int> window_idxs(W21sqr, -1);
2262 for (
int Acy = Acy0; Acy <= 0; Acy++)
2264 int limit_cx = Acy < 0 ? Acx1 : -1;
2266 size_t idx = cx_c + Acx0 +
m_size_x * (cy_c + Acy);
2267 int idx_c_in_idx = -Acy * W21 - Acx0;
2269 int window_idx = Acx0 + W + (Acy + W) * W21;
2271 for (
int Acx = Acx0; Acx <= limit_cx; Acx++)
2274 const double cov_i_c =
m_stackedCov(idx, idx_c_in_idx);
2277 m_map[idx].kf_mean() += yk * sk_1 * cov_i_c;
2280 cross_covs_c_i[window_idx] = cov_i_c;
2281 window_idxs[window_idx++] = idx;
2289 for (
int Acy = 0; Acy <= Acy1; Acy++)
2291 int start_cx = Acy > 0 ? Acx0 : 0;
2293 size_t idx = cx_c + start_cx +
m_size_x * (cy_c + Acy);
2297 (W + 1) + (Acy - 1) * W21 + (start_cx + W);
2301 int window_idx = start_cx + W + (Acy + W) * W21;
2303 for (
int Acx = start_cx; Acx <= Acx1; Acx++)
2305 ASSERT_(idx_i_in_c >= 0 && idx_i_in_c <
int(K));
2308 m_map[idx].kf_mean() += yk * sk_1 * cov_i_c;
2311 cross_covs_c_i[window_idx] = cov_i_c;
2312 window_idxs[window_idx++] = idx;
2327 for (
size_t i = 0; i < W21sqr; i++)
2329 const int idx_i = window_idxs[i];
2330 if (idx_i < 0)
continue;
2336 const double cov_c_i = cross_covs_c_i[i];
2338 for (
size_t j = i; j < W21sqr; j++)
2340 const int idx_j = window_idxs[j];
2341 if (idx_j < 0)
continue;
2347 const int Ax = cx_j - cx_i;
2352 const int Ay = cy_j - cy_i;
2355 const double cov_c_j = cross_covs_c_i[j];
2359 idx_j_in_i = Ax + W + (Ay - 1) * W21 + W + 1;
2364 double& cov_to_change =
m_stackedCov(idx_i, idx_j_in_i);
2365 double Delta_cov = cov_c_j * cov_c_i * sk_1;
2366 if (i == j && cov_to_change < Delta_cov)
2368 "Negative variance value appeared! Please increase the "
2369 "size of the window "
2370 "(W).\n(m_insertOptions_common->KF_covSigma=%f)",
2373 cov_to_change -= Delta_cov;
2391 const size_t N =
m_map.size();
2392 for (
size_t i = 0; i < N; i++)
2404 for (
size_t i = 0; i < N; ++i) out_means[i] =
BASE::m_map[i].kf_mean();
2421 for (
size_t i = 0; i < N; ++i)
2440 for (
size_t i = 0; i < N; ++i)
2442 m_map[i].kf_mean() = in_means[i];
2469 "insertObservation() isn't implemented for selected 'mapType'");
2475 const bool update_map,
const bool time_invariant,
2476 const double reading_stddev)
2494 sensorReading, point, update_map, time_invariant,
2495 reading_stddev == .0
2502 "insertObservation() isn't implemented for selected 'mapType'");
2511 const bool update_map,
const bool time_invariant,
2512 const double reading_information)
2517 const int cellIdx =
xy2idx(point.
x, point.
y);
2525 new_obs.
Lambda = reading_information;
2532 catch (
const std::exception& e)
2534 cerr <<
"Exception while Inserting new Observation: " << e.what()
2554 size_t(
m_map.size()) ==
size_t(x_var.
size()));
2557 for (
size_t j = 0; j <
m_map.size(); j++)
2561 : std::sqrt(x_var[j]);
2562 m_map[j].gmrf_mean() += x_incr[j];
2575 for (
auto ito = obs.begin(); ito != obs.end();)
2577 if (!ito->time_invariant)
2584 if (ito->Lambda < 0)
2587 ito = obs.erase(ito);
2598 size_t cxo_max,
size_t cyo_min,
size_t cyo_max,
const size_t seed_cxo,
2599 const size_t seed_cyo,
const size_t objective_cxo,
2600 const size_t objective_cyo)
2606 cxo_min = max(cxo_min, (
size_t)0);
2607 cxo_max = min(cxo_max, (
size_t)m_Ocgridmap->
getSizeX() - 1);
2608 cyo_min = max(cyo_min, (
size_t)0);
2609 cyo_max = min(cyo_max, (
size_t)m_Ocgridmap->
getSizeY() - 1);
2615 if ((seed_cxo < cxo_min) || (seed_cxo >= cxo_max) || (seed_cyo < cyo_min) ||
2616 (seed_cyo >= cyo_max))
2621 if ((objective_cxo < cxo_min) || (objective_cxo >= cxo_max) ||
2622 (objective_cyo < cyo_min) || (objective_cyo >= cyo_max))
2629 if ((m_Ocgridmap->
getCell(seed_cxo, seed_cyo) < 0.5) !=
2630 (m_Ocgridmap->
getCell(objective_cxo, objective_cyo) < 0.5))
2638 cxo_max - cxo_min + 1, cyo_max - cyo_min + 1);
2645 matExp(seed_cxo - cxo_min, seed_cyo - cyo_min) = 1;
2650 while (seedsOld < seedsNew)
2652 seedsOld = seedsNew;
2654 for (
int col = 0; col < matExp.
cols(); col++)
2656 for (
int row = 0; row < matExp.
rows(); row++)
2659 if (matExp(row, col) == 1)
2661 matExp(row, col) = 2;
2663 for (
int i = -1; i <= 1; i++)
2665 for (
int j = -1; j <= 1; j++)
2668 if ((
int(row) + j >= 0) &&
2669 (
int(row) + j <=
int(matExp.
rows() - 1)) &&
2670 (
int(col) + i >= 0) &&
2671 (
int(col) + i <=
int(matExp.
cols()) - 1))
2673 if (!((i == 0 && j == 0) ||
2674 !(matExp(row + j, col + i) == 0)))
2678 row + cxo_min, col + cyo_min) <
2682 col + i + cyo_min) < 0.5))
2684 if ((row + j + cxo_min ==
2686 (col + i + cyo_min ==
2693 matExp(row + j, col + i) = 1;
2720 return m_parent->m_map[this->node_id].gmrf_mean() - this->obsValue;
2724 return this->Lambda;
2733 return m_parent->m_map[this->node_id_i].gmrf_mean() -
2734 m_parent->m_map[this->node_id_j].gmrf_mean();
2738 return this->Lambda;
2741 double& dr_dx_i,
double& dr_dx_j)
const