12 #include <mrpt/config.h>
43 return img.saveToFile(file);
46 std::cerr <<
"[COccupancyGridMap2D::saveAsBitmapFile] Doing nothing, since "
47 "MRPT was built without OpenCV.\n";
61 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
67 out << size_x << size_y << x_min << x_max << y_min << y_max << resolution;
68 ASSERT_(size_x * size_y == map.size());
70 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
71 out.WriteBuffer(&map[0],
sizeof(map[0]) * size_x * size_y);
73 out.WriteBufferFixEndianness(&map[0], size_x * size_y);
77 out << insertionOptions.mapAltitude << insertionOptions.useMapAltitude
78 << insertionOptions.maxDistanceInsertion
79 << insertionOptions.maxOccupancyUpdateCertainty
80 << insertionOptions.considerInvalidRangesAsFreeSpace
81 << insertionOptions.decimation << insertionOptions.horizontalTolerance;
84 out << (int32_t)likelihoodOptions.likelihoodMethod
85 << likelihoodOptions.LF_stdHit << likelihoodOptions.LF_zHit
86 << likelihoodOptions.LF_zRandom << likelihoodOptions.LF_maxRange
87 << likelihoodOptions.LF_decimation
88 << likelihoodOptions.LF_maxCorrsDistance
89 << likelihoodOptions.LF_alternateAverageMethod
90 << likelihoodOptions.MI_exponent << likelihoodOptions.MI_skip_rays
91 << likelihoodOptions.MI_ratio_max_distance
92 << likelihoodOptions.rayTracing_useDistanceFilter
93 << likelihoodOptions.rayTracing_decimation
94 << likelihoodOptions.rayTracing_stdHit
95 << likelihoodOptions.consensus_takeEachRange
96 << likelihoodOptions.consensus_pow << likelihoodOptions.OWA_weights
97 << likelihoodOptions.enableLikelihoodCache;
100 out << genericMapParams;
103 out << insertionOptions.CFD_features_gaussian_size
104 << insertionOptions.CFD_features_median_size;
107 out << insertionOptions.wideningBeamsWithDistance;
125 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
126 const uint8_t MyBitsPerCell = 8;
128 const uint8_t MyBitsPerCell = 16;
131 uint8_t bitsPerCellStream;
135 in >> bitsPerCellStream;
140 uint32_t new_size_x, new_size_y;
141 float new_x_min, new_x_max, new_y_min, new_y_max;
142 float new_resolution;
144 in >> new_size_x >> new_size_y >> new_x_min >> new_x_max >>
145 new_y_min >> new_y_max >> new_resolution;
148 new_x_min, new_x_max, new_y_min, new_y_max, new_resolution,
151 ASSERT_(size_x * size_y == map.size());
153 if (bitsPerCellStream == MyBitsPerCell)
156 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
157 in.
ReadBuffer(&map[0],
sizeof(map[0]) * map.size());
165 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
167 ASSERT_(bitsPerCellStream == 16);
168 std::vector<uint16_t> auxMap(map.size());
169 in.
ReadBuffer(&auxMap[0],
sizeof(auxMap[0]) * auxMap.size());
171 size_t i, N = map.size();
172 auto* ptrTrg = (uint8_t*)&map[0];
173 const auto* ptrSrc = (
const uint16_t*)&auxMap[0];
174 for (i = 0; i < N; i++) *ptrTrg++ = (*ptrSrc++) >> 8;
177 ASSERT_(bitsPerCellStream == 8);
178 std::vector<uint8_t> auxMap(map.size());
179 in.
ReadBuffer(&auxMap[0],
sizeof(auxMap[0]) * auxMap.size());
181 size_t i, N = map.size();
182 uint16_t* ptrTrg = (uint16_t*)&map[0];
183 const uint8_t* ptrSrc = (
const uint8_t*)&auxMap[0];
184 for (i = 0; i < N; i++) *ptrTrg++ = (*ptrSrc++) << 8;
192 size_t i, N = map.size();
194 for (i = 0; i < N; i++)
204 m_likelihoodCacheOutDated =
true;
209 in >> insertionOptions.mapAltitude >>
210 insertionOptions.useMapAltitude >>
211 insertionOptions.maxDistanceInsertion >>
212 insertionOptions.maxOccupancyUpdateCertainty >>
213 insertionOptions.considerInvalidRangesAsFreeSpace >>
214 insertionOptions.decimation >>
215 insertionOptions.horizontalTolerance;
220 likelihoodOptions.likelihoodMethod =
222 in >> likelihoodOptions.LF_stdHit >>
223 likelihoodOptions.LF_zHit >> likelihoodOptions.LF_zRandom >>
224 likelihoodOptions.LF_maxRange >>
225 likelihoodOptions.LF_decimation >>
226 likelihoodOptions.LF_maxCorrsDistance >>
227 likelihoodOptions.LF_alternateAverageMethod >>
228 likelihoodOptions.MI_exponent >>
229 likelihoodOptions.MI_skip_rays >>
230 likelihoodOptions.MI_ratio_max_distance >>
231 likelihoodOptions.rayTracing_useDistanceFilter >>
232 likelihoodOptions.rayTracing_decimation >>
233 likelihoodOptions.rayTracing_stdHit >>
234 likelihoodOptions.consensus_takeEachRange >>
235 likelihoodOptions.consensus_pow >>
236 likelihoodOptions.OWA_weights >>
237 likelihoodOptions.enableLikelihoodCache;
241 in >> genericMapParams;
244 bool disableSaveAs3DObject;
245 in >> disableSaveAs3DObject;
246 genericMapParams.enableSaveAs3DObject =
247 !disableSaveAs3DObject;
253 in >> insertionOptions.CFD_features_gaussian_size >>
254 insertionOptions.CFD_features_median_size;
259 in >> insertionOptions.wideningBeamsWithDistance;
269 const std::string& file,
float res,
const TPoint2D& origin)
274 if (!imgFl.loadFromFile(file, 0))
return false;
277 return loadFromBitmap(imgFl, res, origin);
289 m_likelihoodCacheOutDated =
true;
294 if (size_x != bmpWidth || size_y != bmpHeight)
296 auto origin = origin_;
298 if (origin.x == std::numeric_limits<double>::max())
305 float new_x_max = (imgFl.
getWidth() - origin.x) * res;
306 float new_x_min = -origin.x * res;
307 float new_y_max = (imgFl.
getHeight() - origin.y) * res;
308 float new_y_min = -origin.y * res;
310 setSize(new_x_min, new_x_max, new_y_min, new_y_max, res);
314 for (
size_t x = 0; x < bmpWidth; x++)
315 for (
size_t y = 0; y < bmpHeight; y++)
317 float f = imgFl.
getAsFloat(x, bmpHeight - 1 - y);
318 f = std::max(0.01f, f);
319 f = std::min(0.99f, f);
339 unsigned int i, n, Ay1, Ay2;
346 unsigned int lx1 = img1.getWidth();
347 unsigned int ly1 = img1.getHeight();
349 unsigned int lx2 = img2.getWidth();
350 unsigned int ly2 = img2.getHeight();
356 Ay2 = (ly1 - ly2) / 2;
361 Ay1 = (ly2 - ly1) / 2;
376 TColor lineColor = TColor::black();
377 for (i = 0; i < n; i++)
380 px = m1->
x2idx(corrs[i].this_x);
381 py = Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y);
382 img.
rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
383 img.
rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
386 px = lx1 + 1 + m2->
x2idx(corrs[i].other_x);
387 py = Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y);
388 img.
rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
389 img.
rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
394 for (i = 0; i < n; i++)
402 m1->
x2idx(corrs[i].this_x),
404 Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y),
405 lx1 + 1 + m2->
x2idx(corrs[i].other_x),
406 Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y), lineColor);
426 unsigned int i, Ay1, Ay2;
429 lineColor = TColor::red();
449 Ay2 = (ly1 - ly2) / 2;
454 Ay1 = (ly2 - ly1) / 2;
459 emf.drawImage(0, Ay1, img1);
460 emf.drawImage(lx1 + 1, Ay2, img2);
464 const unsigned int n = corrs.size();
465 lineColor = TColor::black();
466 for (i = 0; i < n; i++)
469 px = m1->
x2idx(corrs[i].this_x);
470 py = Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y);
471 emf.rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
472 emf.rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
475 px = lx1 + 1 + m2->
x2idx(corrs[i].other_x);
476 py = Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y);
477 emf.rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
478 emf.rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
503 for (i = 0; i < n; i++)
508 m1->
x2idx(corrs[i].this_x) - 10,
509 Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y) - 25, str,
513 lx1 + 1 + m2->
x2idx(corrs[i].other_x) - 10,
514 Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y) - 25, str,
524 const std::string& filNamePrefix)
const
526 std::string fil(filNamePrefix + std::string(
".png"));
527 saveAsBitmapFile(fil);
529 fil = filNamePrefix + std::string(
"_limits.txt");
531 LIMITS(0, 0) = x_min;
532 LIMITS(0, 1) = x_max;
533 LIMITS(0, 2) = y_min;
534 LIMITS(0, 3) = y_max;
537 "% Grid limits: [x_min x_max y_min y_max]\n");