Main MRPT website > C++ reference for MRPT 1.5.6
COccupancyGridMap2D_getAs.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-2017, 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 
14 #include <mrpt/system/os.h>
15 #include <mrpt/utils/round.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::maps;
21 using namespace mrpt::obs;
22 using namespace mrpt::utils;
23 using namespace mrpt::poses;
24 using namespace std;
25 
26 
27 
28 /*---------------------------------------------------------------
29  getAsImage
30  ---------------------------------------------------------------*/
33  bool verticalFlip,
34  bool forceRGB,
35  bool tricolor ) const
36 {
37  if (!tricolor)
38  {
39  if (!forceRGB)
40  { // 8bit gray-scale
41  img.resize(size_x,size_y,1,true); //verticalFlip);
42  const cellType *srcPtr = &map[0];
43  unsigned char *destPtr;
44  for (unsigned int y=0;y<size_y;y++)
45  {
46  if (!verticalFlip)
47  destPtr = img(0,size_y-1-y);
48  else destPtr = img(0,y);
49  for (unsigned int x=0;x<size_x;x++)
50  {
51  *destPtr++ = l2p_255(*srcPtr++);
52  }
53  }
54  }
55  else
56  { // 24bit RGB:
57  img.resize(size_x,size_y,3,true); //verticalFlip);
58  const cellType *srcPtr = &map[0];
59  unsigned char *destPtr;
60  for (unsigned int y=0;y<size_y;y++)
61  {
62  if (!verticalFlip)
63  destPtr = img(0,size_y-1-y);
64  else destPtr = img(0,y);
65  for (unsigned int x=0;x<size_x;x++)
66  {
67  uint8_t c = l2p_255(*srcPtr++);
68  *destPtr++ = c;
69  *destPtr++ = c;
70  *destPtr++ = c;
71  }
72  }
73  }
74  }
75  else
76  {
77  // TRICOLOR: 0, 0.5, 1
78  if (!forceRGB)
79  { // 8bit gray-scale
80  img.resize(size_x,size_y,1,true); //verticalFlip);
81  const cellType *srcPtr = &map[0];
82  unsigned char *destPtr;
83  for (unsigned int y=0;y<size_y;y++)
84  {
85  if (!verticalFlip)
86  destPtr = img(0,size_y-1-y);
87  else destPtr = img(0,y);
88  for (unsigned int x=0;x<size_x;x++)
89  {
90  uint8_t c = l2p_255(*srcPtr++);
91  if (c<120)
92  c=0;
93  else if (c>136)
94  c=255;
95  else c = 127;
96  *destPtr++ = c;
97  }
98  }
99  }
100  else
101  { // 24bit RGB:
102  img.resize(size_x,size_y,3,true); //verticalFlip);
103  const cellType *srcPtr = &map[0];
104  unsigned char *destPtr;
105  for (unsigned int y=0;y<size_y;y++)
106  {
107  if (!verticalFlip)
108  destPtr = img(0,size_y-1-y);
109  else 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 c = 127;
118 
119  *destPtr++ = c;
120  *destPtr++ = c;
121  *destPtr++ = c;
122  }
123  }
124  }
125  }
126 }
127 
128 /*---------------------------------------------------------------
129  getAsImageFiltered
130  ---------------------------------------------------------------*/
133  bool verticalFlip,
134  bool forceRGB ) const
135 {
136  getAsImage(img,verticalFlip,forceRGB);
137 
138  // Do filtering to improve the noisy peaks in grids:
139  // ----------------------------------------------------
140 #if 0
141  CTicTac t;
142 #endif
143  if (insertionOptions.CFD_features_gaussian_size!=0) img.filterGaussianInPlace( round( insertionOptions.CFD_features_gaussian_size ) );
144  if (insertionOptions.CFD_features_median_size!=0) img.filterMedianInPlace( round( insertionOptions.CFD_features_median_size ) );
145 #if 0
146  cout << "[COccupancyGridMap2D::getAsImageFiltered] Filtered in: " << t.Tac()*1000 << " ms" << endl;
147 #endif
148 }
149 
150 
151 
152 /*---------------------------------------------------------------
153  getAs3DObject
154  ---------------------------------------------------------------*/
155 void COccupancyGridMap2D::getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outSetOfObj ) const
156 {
157  if (!genericMapParams.enableSaveAs3DObject) return;
158 
159  MRPT_START
160 
161  opengl::CTexturedPlanePtr outObj = opengl::CTexturedPlane::Create();
162 
163  outObj->setPlaneCorners(x_min,x_max,y_min,y_max);
164 
165  outObj->setLocation(0,0, insertionOptions.mapAltitude );
166 
167  // Create the color & transparecy (alpha) images:
168  CImage imgColor(size_x,size_y,1);
169  CImage imgTrans(size_x,size_y,1);
170 
171 
172  const cellType *srcPtr = &map[0];
173 
174  for (unsigned int y=0;y<size_y;y++)
175  {
176  unsigned char *destPtr_color = imgColor(0,y);
177  unsigned char *destPtr_trans = imgTrans(0,y);
178  for (unsigned int x=0;x<size_x;x++)
179  {
180  uint8_t cell255 = l2p_255(*srcPtr++);
181  *destPtr_color++ = cell255;
182 
183  int8_t auxC = (int8_t)((signed short)cell255)-127;
184  *destPtr_trans++ = auxC>0 ? (auxC << 1) : ((-auxC) << 1);
185  }
186  }
187 
188  outObj->assignImage_fast( imgColor,imgTrans );
189  outSetOfObj->insert( outObj );
190 
191  MRPT_END
192 }
193 
194 
195 
196 /** Get a point cloud with all (border) occupied cells as points */
197 void COccupancyGridMap2D::getAsPointCloud( mrpt::maps::CSimplePointsMap &pm, const float occup_threshold ) const
198 {
199  pm.clear();
200  pm.reserve(1000);
201 
202  // for all rows in the gridmap
203  for (size_t i=1; i+1<size_x; i++)
204  {
205  // for all columns in the gridmap
206  for (size_t j=1; j+1<size_y; j++)
207  {
208  //if there is an obstacle and *it is a borderline*:
209  bool is_surrounded = true;
210  for (int di=-1;di<=1 && is_surrounded;di++)
211  for (int dj=-1;dj<=1 && is_surrounded;dj++)
212  if ((di!=0 || dj!=0) && getCell(i+di,j+dj)>occup_threshold)
213  is_surrounded=false;
214 
215  if (getCell(i,j)<occup_threshold && !is_surrounded)
216  pm.insertPoint(idx2x(i), idx2y(j));
217  }
218  }
219 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void getAsPointCloud(mrpt::maps::CSimplePointsMap &pm, const float occup_threshold=0.5f) const
Get a point cloud with all (border) occupied cells as points.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
const GLfloat * c
Definition: glew.h:10088
void resize(unsigned int width, unsigned int height, TImageChannels nChannels, bool originTopLeft)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
Definition: CImage.h:209
GLdouble GLdouble t
Definition: glew.h:1303
static CTexturedPlanePtr Create()
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:42
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...
void getAsImageFiltered(utils::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:43
GLint GLvoid * img
Definition: glew.h:1290
#define MRPT_END
void getAsImage(utils::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 implements a high-performance stopwatch.
Definition: CTicTac.h:24
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncert...
void filterMedianInPlace(int W=3)
Filter the image with a Median filter with a window size WxH, replacing "this" image by the filtered ...
Definition: CImage.cpp:2149
#define MRPT_START
void filterGaussianInPlace(int W=3, int H=3)
Filter the image with a Gaussian filter with a window size WxH, returning the filtered image in out_i...
Definition: CImage.cpp:2199
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
virtual void reserve(size_t newLength) MRPT_OVERRIDE
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
int16_t cellType
The type of the map cells:



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018