MRPT  1.9.9
COccupancyGridMap2D_getAs.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/round.h>
17 #include <mrpt/system/os.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::maps;
21 using namespace mrpt::obs;
22 using namespace mrpt::poses;
23 using namespace mrpt::img;
24 using namespace std;
25 
26 /*---------------------------------------------------------------
27  getAsImage
28  ---------------------------------------------------------------*/
30  CImage& img, bool verticalFlip, bool forceRGB, bool tricolor) const
31 {
32  if (!tricolor)
33  {
34  if (!forceRGB)
35  { // 8bit gray-scale
36  img.resize(size_x, size_y, mrpt::img::CH_GRAY);
37  const cellType* srcPtr = &map[0];
38  unsigned char* destPtr;
39  for (unsigned int y = 0; y < size_y; y++)
40  {
41  if (!verticalFlip)
42  destPtr = img(0, size_y - 1 - y);
43  else
44  destPtr = img(0, y);
45  for (unsigned int x = 0; x < size_x; x++)
46  {
47  *destPtr++ = l2p_255(*srcPtr++);
48  }
49  }
50  }
51  else
52  { // 24bit RGB:
53  img.resize(size_x, size_y, mrpt::img::CH_RGB);
54  const cellType* srcPtr = &map[0];
55  unsigned char* destPtr;
56  for (unsigned int y = 0; y < size_y; y++)
57  {
58  if (!verticalFlip)
59  destPtr = img(0, size_y - 1 - y);
60  else
61  destPtr = img(0, y);
62  for (unsigned int x = 0; x < size_x; x++)
63  {
64  uint8_t c = l2p_255(*srcPtr++);
65  *destPtr++ = c;
66  *destPtr++ = c;
67  *destPtr++ = c;
68  }
69  }
70  }
71  }
72  else
73  {
74  // TRICOLOR: 0, 0.5, 1
75  if (!forceRGB)
76  { // 8bit gray-scale
77  img.resize(size_x, size_y, mrpt::img::CH_GRAY);
78  const cellType* srcPtr = &map[0];
79  unsigned char* destPtr;
80  for (unsigned int y = 0; y < size_y; y++)
81  {
82  if (!verticalFlip)
83  destPtr = img(0, size_y - 1 - y);
84  else
85  destPtr = img(0, y);
86  for (unsigned int x = 0; x < size_x; x++)
87  {
88  uint8_t c = l2p_255(*srcPtr++);
89  if (c < 120)
90  c = 0;
91  else if (c > 136)
92  c = 255;
93  else
94  c = 127;
95  *destPtr++ = c;
96  }
97  }
98  }
99  else
100  { // 24bit RGB:
101  img.resize(size_x, size_y, mrpt::img::CH_RGB);
102  const cellType* srcPtr = &map[0];
103  unsigned char* destPtr;
104  for (unsigned int y = 0; y < size_y; y++)
105  {
106  if (!verticalFlip)
107  destPtr = img(0, size_y - 1 - y);
108  else
109  destPtr = img(0, y);
110  for (unsigned int x = 0; x < size_x; x++)
111  {
112  uint8_t c = l2p_255(*srcPtr++);
113  if (c < 120)
114  c = 0;
115  else if (c > 136)
116  c = 255;
117  else
118  c = 127;
119 
120  *destPtr++ = c;
121  *destPtr++ = c;
122  *destPtr++ = c;
123  }
124  }
125  }
126  }
127 }
128 
129 /*---------------------------------------------------------------
130  getAsImageFiltered
131  ---------------------------------------------------------------*/
133  CImage& img, bool verticalFlip, bool forceRGB) const
134 {
135  getAsImage(img, verticalFlip, forceRGB);
136 
137  // Do filtering to improve the noisy peaks in grids:
138  if (insertionOptions.CFD_features_gaussian_size != 0)
139  img.filterGaussian(
140  img, round(insertionOptions.CFD_features_gaussian_size));
141  if (insertionOptions.CFD_features_median_size != 0)
142  img.filterMedian(img, round(insertionOptions.CFD_features_median_size));
143 }
144 
145 /*---------------------------------------------------------------
146  getAs3DObject
147  ---------------------------------------------------------------*/
149  mrpt::opengl::CSetOfObjects::Ptr& outSetOfObj) const
150 {
151  if (!genericMapParams.enableSaveAs3DObject) return;
152 
153  MRPT_START
154 
156  mrpt::make_aligned_shared<opengl::CTexturedPlane>();
157 
158  outObj->setPlaneCorners(x_min, x_max, y_min, y_max);
159 
160  outObj->setLocation(0, 0, insertionOptions.mapAltitude);
161 
162  // Create the color & transparecy (alpha) images:
163  CImage imgColor(size_x, size_y, mrpt::img::CH_GRAY);
164  CImage imgTrans(size_x, size_y, mrpt::img::CH_GRAY);
165 
166  const cellType* srcPtr = &map[0];
167 
168  for (unsigned int y = 0; y < size_y; y++)
169  {
170  unsigned char* destPtr_color = imgColor(0, y);
171  unsigned char* destPtr_trans = imgTrans(0, y);
172  for (unsigned int x = 0; x < size_x; x++)
173  {
174  uint8_t cell255 = l2p_255(*srcPtr++);
175  *destPtr_color++ = cell255;
176 
177  int8_t auxC = (int8_t)((signed short)cell255) - 127;
178  *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
179  }
180  }
181 
182  outObj->assignImage_fast(imgColor, imgTrans);
183  outSetOfObj->insert(outObj);
184 
185  MRPT_END
186 }
187 
188 /** Get a point cloud with all (border) occupied cells as points */
190  mrpt::maps::CSimplePointsMap& pm, const float occup_threshold) const
191 {
192  pm.clear();
193  pm.reserve(1000);
194 
195  // for all rows in the gridmap
196  for (size_t i = 1; i + 1 < size_x; i++)
197  {
198  // for all columns in the gridmap
199  for (size_t j = 1; j + 1 < size_y; j++)
200  {
201  // if there is an obstacle and *it is a borderline*:
202  bool is_surrounded = true;
203  for (int di = -1; di <= 1 && is_surrounded; di++)
204  for (int dj = -1; dj <= 1 && is_surrounded; dj++)
205  if ((di != 0 || dj != 0) &&
206  getCell(i + di, j + dj) > occup_threshold)
207  is_surrounded = false;
208 
209  if (getCell(i, j) < occup_threshold && !is_surrounded)
210  pm.insertPoint(idx2x(i), idx2y(j));
211  }
212  }
213 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
#define MRPT_START
Definition: exceptions.h:282
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
signed char int8_t
Definition: rptypes.h:43
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:625
STL namespace.
void getAsImageFiltered(img::CImage &img, bool verticalFlip=false, bool forceRGB=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
unsigned char uint8_t
Definition: rptypes.h:44
const GLubyte * c
Definition: glext.h:6406
GLint GLvoid * img
Definition: glext.h:3769
This namespace contains representation of robot actions and observations.
void getAsPointCloud(mrpt::maps::CSimplePointsMap &pm, const float occup_threshold=0.5f) const
Get a point cloud with all (border) occupied cells as points.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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 is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncert...
#define MRPT_END
Definition: exceptions.h:286
OccGridCellTraits::cellType cellType
The type of the map cells:
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019