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;
146 in >> new_size_x >> new_size_y >> new_x_min >> new_x_max >>
147 new_y_min >> new_y_max >> new_resolution;
150 new_x_min, new_x_max, new_y_min, new_y_max, new_resolution,
153 ASSERT_(size_x * size_y == map.size());
155 if (bitsPerCellStream == MyBitsPerCell)
158 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS 159 in.ReadBuffer(&map[0],
sizeof(map[0]) * map.size());
161 in.ReadBufferFixEndianness(&map[0], map.size());
167 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS 169 ASSERT_(bitsPerCellStream == 16);
170 std::vector<uint16_t> auxMap(map.size());
171 in.ReadBuffer(&auxMap[0],
sizeof(auxMap[0]) * auxMap.size());
173 size_t i, N = map.size();
174 auto* ptrTrg = (uint8_t*)&map[0];
175 const auto* ptrSrc = (
const uint16_t*)&auxMap[0];
176 for (i = 0; i < N; i++) *ptrTrg++ = (*ptrSrc++) >> 8;
179 ASSERT_(bitsPerCellStream == 8);
180 std::vector<uint8_t> auxMap(map.size());
181 in.ReadBuffer(&auxMap[0],
sizeof(auxMap[0]) * auxMap.size());
183 size_t i, N = map.size();
184 uint16_t* ptrTrg = (uint16_t*)&map[0];
185 const uint8_t* ptrSrc = (
const uint8_t*)&auxMap[0];
186 for (i = 0; i < N; i++) *ptrTrg++ = (*ptrSrc++) << 8;
194 size_t i, N = map.size();
196 for (i = 0; i < N; i++)
206 m_likelihoodCacheOutDated =
true;
211 in >> insertionOptions.mapAltitude >>
212 insertionOptions.useMapAltitude >>
213 insertionOptions.maxDistanceInsertion >>
214 insertionOptions.maxOccupancyUpdateCertainty >>
215 insertionOptions.considerInvalidRangesAsFreeSpace >>
216 insertionOptions.decimation >>
217 insertionOptions.horizontalTolerance;
222 likelihoodOptions.likelihoodMethod =
224 in >> likelihoodOptions.LF_stdHit >>
225 likelihoodOptions.LF_zHit >> likelihoodOptions.LF_zRandom >>
226 likelihoodOptions.LF_maxRange >>
227 likelihoodOptions.LF_decimation >>
228 likelihoodOptions.LF_maxCorrsDistance >>
229 likelihoodOptions.LF_alternateAverageMethod >>
230 likelihoodOptions.MI_exponent >>
231 likelihoodOptions.MI_skip_rays >>
232 likelihoodOptions.MI_ratio_max_distance >>
233 likelihoodOptions.rayTracing_useDistanceFilter >>
234 likelihoodOptions.rayTracing_decimation >>
235 likelihoodOptions.rayTracing_stdHit >>
236 likelihoodOptions.consensus_takeEachRange >>
237 likelihoodOptions.consensus_pow >>
238 likelihoodOptions.OWA_weights >>
239 likelihoodOptions.enableLikelihoodCache;
243 in >> genericMapParams;
246 bool disableSaveAs3DObject;
247 in >> disableSaveAs3DObject;
248 genericMapParams.enableSaveAs3DObject =
249 !disableSaveAs3DObject;
255 in >> insertionOptions.CFD_features_gaussian_size >>
256 insertionOptions.CFD_features_median_size;
261 in >> insertionOptions.wideningBeamsWithDistance;
276 if (!imgFl.loadFromFile(file, 0))
return false;
279 return loadFromBitmap(imgFl,
res, origin);
291 m_likelihoodCacheOutDated =
true;
296 if (size_x != bmpWidth || size_y != bmpHeight)
298 auto origin = origin_;
300 if (origin.x == std::numeric_limits<double>::max())
307 float new_x_max = (imgFl.
getWidth() - origin.x) *
res;
308 float new_x_min = -origin.x *
res;
310 float new_y_min = -origin.y *
res;
312 setSize(new_x_min, new_x_max, new_y_min, new_y_max,
res);
316 for (
size_t x = 0;
x < bmpWidth;
x++)
317 for (
size_t y = 0;
y < bmpHeight;
y++)
320 f = std::max(0.01f, f);
321 f = std::min(0.99f, f);
341 unsigned int i,
n, Ay1, Ay2;
348 unsigned int lx1 = img1.getWidth();
349 unsigned int ly1 = img1.getHeight();
351 unsigned int lx2 = img2.getWidth();
352 unsigned int ly2 = img2.getHeight();
358 Ay2 = (ly1 - ly2) / 2;
363 Ay1 = (ly2 - ly1) / 2;
370 0, 0,
img.getWidth() - 1,
img.getHeight() - 1,
372 img.drawImage(0, Ay1, img1);
373 img.drawImage(lx1 + 1, Ay2, img2);
378 TColor lineColor = TColor::black();
379 for (i = 0; i <
n; i++)
382 px = m1->
x2idx(corrs[i].this_x);
383 py = Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y);
384 img.rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
385 img.rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
388 px = lx1 + 1 + m2->
x2idx(corrs[i].other_x);
389 py = Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y);
390 img.rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
391 img.rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
396 for (i = 0; i <
n; i++)
404 m1->
x2idx(corrs[i].this_x),
406 Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y),
407 lx1 + 1 + m2->
x2idx(corrs[i].other_x),
408 Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y), lineColor);
411 return img.saveToFile(fileName.c_str());
428 unsigned int i, Ay1, Ay2;
431 lineColor = TColor::red();
451 Ay2 = (ly1 - ly2) / 2;
456 Ay1 = (ly2 - ly1) / 2;
461 emf.drawImage(0, Ay1, img1);
462 emf.drawImage(lx1 + 1, Ay2, img2);
466 const unsigned int n = corrs.size();
467 lineColor = TColor::black();
468 for (i = 0; i <
n; i++)
471 px = m1->
x2idx(corrs[i].this_x);
472 py = Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y);
473 emf.rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
474 emf.rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
477 px = lx1 + 1 + m2->
x2idx(corrs[i].other_x);
478 py = Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y);
479 emf.rectangle(px - 10, py - 10, px + 10, py + 10, lineColor);
480 emf.rectangle(px - 11, py - 11, px + 11, py + 11, lineColor);
505 for (i = 0; i <
n; i++)
510 m1->
x2idx(corrs[i].this_x) - 10,
511 Ay1 + ly1 - 1 - m1->
y2idx(corrs[i].this_y) - 25, str,
515 lx1 + 1 + m2->
x2idx(corrs[i].other_x) - 10,
516 Ay2 + ly2 - 1 - m2->
y2idx(corrs[i].other_y) - 25, str,
529 saveAsBitmapFile(fil);
533 LIMITS(0, 0) = x_min;
534 LIMITS(0, 1) = x_max;
535 LIMITS(0, 2) = y_min;
536 LIMITS(0, 3) = y_max;
539 "% Grid limits: [x_min x_max y_min y_max]\n");
A namespace of pseudo-random numbers generators of diferent distributions.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
OccGridCellTraits::cellTypeUnsigned cellTypeUnsigned
TLikelihoodMethod
The type for selecting a likelihood computation method.
size_t getHeight() const override
Returns the height of the image in pixels.
bool loadFromBitmapFile(const std::string &file, float resolution, const mrpt::math::TPoint2D &origin=mrpt::math::TPoint2D(std::numeric_limits< double >::max(), std::numeric_limits< double >::max()))
Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
static bool saveAsEMFTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::tfest::TMatchingPairList &corrs)
Saves a composite image with two gridmaps and numbers for the correspondences between them...
float getAsFloat(unsigned int col, unsigned int row, unsigned int channel) const
Returns the contents of a given pixel at the desired channel, in float format: [0,255]->[0,1] The coordinate origin is pixel(0,0)=top-left corner of the image.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
static bool saveAsBitmapTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::tfest::TMatchingPairList &corrs)
Saves a composite image with two gridmaps and lines representing a set of correspondences between the...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define ASSERT_(f)
Defines an assertion mechanism.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
bool saveAsBitmapFile(const std::string &file) const
Saves the gridmap as a graphical file (BMP,PNG,...).
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
This namespace contains representation of robot actions and observations.
bool loadFromBitmap(const mrpt::img::CImage &img, float resolution, const mrpt::math::TPoint2D &origin=mrpt::math::TPoint2D(std::numeric_limits< double >::max(), std::numeric_limits< double >::max()))
Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
fixed floating point 'f'
A class for storing an occupancy grid map.
void getAsImage(mrpt::img::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
This class is a "CSerializable" wrapper for "CMatrixFloat".
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
mrpt::vision::TStereoCalibResults out
OccGridCellTraits::cellType cellType
The type of the map cells:
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Functions for estimating the optimal transformation between two frames of references given measuremen...
int x2idx(float x) const
Transform a coordinate value into a cell index.
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
A class for storing images as grayscale or RGB bitmaps.