Main MRPT website > C++ reference for MRPT 1.9.9
test.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-2018, 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 <mrpt/maps/COctoMap.h>
11 #include <mrpt/opengl/CGridPlaneXY.h>
12 #include <mrpt/opengl/COctoMapVoxels.h>
13 #include <mrpt/obs/CObservation2DRangeScan.h>
14 #include <mrpt/system/os.h>
15 #include <mrpt/gui/CDisplayWindow3D.h>
16 #include <iostream>
17 
18 //#define HAS_SYSTEM_OCTOMAP
19 
20 #ifdef HAS_SYSTEM_OCTOMAP
21 #include <octomap/octomap.h>
22 #include <octomap/OcTree.h>
23 #endif
24 
25 using namespace mrpt;
26 using namespace mrpt::maps;
27 using namespace mrpt::opengl;
28 using namespace mrpt::math;
29 using namespace mrpt::obs;
30 using namespace std;
31 
32 #define SCAN_SIZE 361
33 
34 float SCAN_RANGES_1[] = {
35  0.910f, 0.900f, 0.910f, 0.900f, 0.900f, 0.890f, 0.890f, 0.880f,
36  0.890f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.870f,
37  0.880f, 0.870f, 0.870f, 0.870f, 0.880f, 0.880f, 0.880f, 0.880f,
38  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
39  0.880f, 0.880f, 0.880f, 0.880f, 0.890f, 0.880f, 0.880f, 0.880f,
40  0.890f, 0.880f, 0.890f, 0.890f, 0.880f, 0.890f, 0.890f, 0.880f,
41  0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.900f, 0.900f,
42  0.900f, 0.900f, 0.900f, 0.910f, 0.910f, 0.910f, 0.910f, 0.920f,
43  0.920f, 0.920f, 0.920f, 0.920f, 0.930f, 0.930f, 0.930f, 0.930f,
44  0.940f, 0.940f, 0.950f, 0.950f, 0.950f, 0.950f, 0.960f, 0.960f,
45  0.970f, 0.970f, 0.970f, 0.980f, 0.980f, 0.990f, 1.000f, 1.000f,
46  1.000f, 1.010f, 1.010f, 1.020f, 1.030f, 1.030f, 1.030f, 1.040f,
47  1.050f, 1.060f, 1.050f, 1.060f, 1.070f, 1.070f, 1.080f, 1.080f,
48  1.090f, 1.100f, 1.110f, 1.120f, 1.120f, 1.130f, 1.140f, 1.140f,
49  1.160f, 1.170f, 1.180f, 1.180f, 1.190f, 1.200f, 1.220f, 1.220f,
50  1.230f, 1.230f, 1.240f, 1.250f, 1.270f, 1.280f, 1.290f, 1.300f,
51  1.320f, 1.320f, 1.350f, 1.360f, 1.370f, 1.390f, 1.410f, 1.410f,
52  1.420f, 1.430f, 1.450f, 1.470f, 1.490f, 1.500f, 1.520f, 1.530f,
53  1.560f, 1.580f, 1.600f, 1.620f, 1.650f, 1.670f, 1.700f, 1.730f,
54  1.750f, 1.780f, 1.800f, 1.830f, 1.850f, 1.880f, 1.910f, 1.940f,
55  1.980f, 2.010f, 2.060f, 2.090f, 2.130f, 2.180f, 2.220f, 2.250f,
56  2.300f, 2.350f, 2.410f, 2.460f, 2.520f, 2.570f, 2.640f, 2.700f,
57  2.780f, 2.850f, 2.930f, 3.010f, 3.100f, 3.200f, 3.300f, 3.390f,
58  3.500f, 3.620f, 3.770f, 3.920f, 4.070f, 4.230f, 4.430f, 4.610f,
59  4.820f, 5.040f, 5.290f, 5.520f, 8.970f, 8.960f, 8.950f, 8.930f,
60  8.940f, 8.930f, 9.050f, 9.970f, 9.960f, 10.110f, 13.960f, 18.870f,
61  19.290f, 81.910f, 20.890f, 48.750f, 48.840f, 48.840f, 19.970f, 19.980f,
62  19.990f, 15.410f, 20.010f, 19.740f, 17.650f, 17.400f, 14.360f, 12.860f,
63  11.260f, 11.230f, 8.550f, 8.630f, 9.120f, 9.120f, 8.670f, 8.570f,
64  7.230f, 7.080f, 7.040f, 6.980f, 6.970f, 5.260f, 5.030f, 4.830f,
65  4.620f, 4.440f, 4.390f, 4.410f, 4.410f, 4.410f, 4.430f, 4.440f,
66  4.460f, 4.460f, 4.490f, 4.510f, 4.540f, 3.970f, 3.820f, 3.730f,
67  3.640f, 3.550f, 3.460f, 3.400f, 3.320f, 3.300f, 3.320f, 3.320f,
68  3.340f, 2.790f, 2.640f, 2.600f, 2.570f, 2.540f, 2.530f, 2.510f,
69  2.490f, 2.490f, 2.480f, 2.470f, 2.460f, 2.460f, 2.460f, 2.450f,
70  2.450f, 2.450f, 2.460f, 2.460f, 2.470f, 2.480f, 2.490f, 2.490f,
71  2.520f, 2.510f, 2.550f, 2.570f, 2.610f, 2.640f, 2.980f, 3.040f,
72  3.010f, 2.980f, 2.940f, 2.920f, 2.890f, 2.870f, 2.830f, 2.810f,
73  2.780f, 2.760f, 2.740f, 2.720f, 2.690f, 2.670f, 2.650f, 2.630f,
74  2.620f, 2.610f, 2.590f, 2.560f, 2.550f, 2.530f, 2.510f, 2.500f,
75  2.480f, 2.460f, 2.450f, 2.430f, 2.420f, 2.400f, 2.390f, 2.380f,
76  2.360f, 2.350f, 2.340f, 2.330f, 2.310f, 2.300f, 2.290f, 2.280f,
77  2.270f, 2.260f, 2.250f, 2.240f, 2.230f, 2.230f, 2.220f, 2.210f,
78  2.200f, 2.190f, 2.180f, 2.170f, 1.320f, 1.140f, 1.130f, 1.130f,
79  1.120f, 1.120f, 1.110f, 1.110f, 1.110f, 1.110f, 1.100f, 1.110f,
80  1.100f};
81 char SCAN_VALID_1[] = {
82  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
83  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
84  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
85  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
86  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
87  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
88  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
89  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
90  1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
91  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
92  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
93  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
94  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
95  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
96  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
97 
98 // ------------------------------------------------------
99 // TestOctoMap
100 // ------------------------------------------------------
101 void TestOctoMap()
102 {
103  COctoMap map(0.2);
104 
105  if (0)
106  {
107  // Manually update voxels:
108  map.updateVoxel(1, 1, 1, true); // integrate 'occupied' measurement
109 
110  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
111  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
112  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
113 
114  map.updateVoxel(-1, -1, 1, false); // integrate 'occupied' measurement
115 
116  double occup;
117  bool is_mapped;
118  mrpt::math::TPoint3D pt;
119 
120  pt = mrpt::math::TPoint3D(1, 1, 1);
121  is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
122  cout << "pt: " << pt << " is mapped?: " << (is_mapped ? "YES" : "NO")
123  << " occupancy: " << occup << endl;
124 
125  pt = mrpt::math::TPoint3D(-1, -1, 1);
126  is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
127  cout << "pt: " << pt << " is mapped?: " << (is_mapped ? "YES" : "NO")
128  << " occupancy: " << occup << endl;
129  }
130 
131  // Insert 2D scan:
132  {
133  CObservation2DRangeScan scan1;
134  scan1.aperture = M_PIf;
135  scan1.rightToLeft = true;
136  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCAN_SIZE);
137  scan1.loadFromVectors(SCAN_SIZE, SCAN_RANGES_1, SCAN_VALID_1);
138  map.insertObservation(&scan1);
139  }
140 
141  mrpt::gui::CDisplayWindow3D win("OctoMap demo", 640, 480);
142 
143  mrpt::opengl::COctoMapVoxels::Ptr gl_map =
144  mrpt::make_aligned_shared<mrpt::opengl::COctoMapVoxels>();
145 
146  {
147  mrpt::opengl::COpenGLScene::Ptr& scene = win.get3DSceneAndLock();
148 
149  {
150  mrpt::opengl::CGridPlaneXY::Ptr gl_grid =
151  mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>(
152  -20, 20, -20, 20, 0, 1);
153  gl_grid->setColor_u8(mrpt::img::TColor(0x80, 0x80, 0x80));
154  scene->insert(gl_grid);
155  }
156 
157  map.renderingOptions.generateGridLines = true;
158  map.getAsOctoMapVoxels(*gl_map);
159 
160  gl_map->showGridLines(false);
161  gl_map->showVoxels(VOXEL_SET_OCCUPIED, true);
162  gl_map->showVoxels(VOXEL_SET_FREESPACE, true);
163  scene->insert(gl_map);
164 
165  win.unlockAccess3DScene();
166  }
167 
168 // Go through voxels:
169 #ifdef HAS_SYSTEM_OCTOMAP
170  {
171  const auto& om = map.getOctomap<octomap::OcTree>();
172  for (auto it = om.begin_leafs(); it != om.end_leafs(); ++it)
173  {
174  const octomap::point3d pt = it.getCoordinate();
175  cout << "pt: " << pt << " -> occupancy = " << it->getOccupancy()
176  << endl;
177  }
178  }
179 #endif
180 
181  cout << "Close the window to exit" << endl;
182 
183  bool update_msg = true;
184 
185  while (win.isOpen())
186  {
187  if (win.keyHit())
188  {
189  const unsigned int k = win.getPushedKey();
190 
191  switch (k)
192  {
193  case 'g':
194  case 'G':
195  gl_map->showGridLines(!gl_map->areGridLinesVisible());
196  break;
197  case 'f':
198  case 'F':
199  gl_map->showVoxels(
200  VOXEL_SET_FREESPACE,
201  !gl_map->areVoxelsVisible(VOXEL_SET_FREESPACE));
202  break;
203  case 'o':
204  case 'O':
205  gl_map->showVoxels(
206  VOXEL_SET_OCCUPIED,
207  !gl_map->areVoxelsVisible(VOXEL_SET_OCCUPIED));
208  break;
209  case 'l':
210  case 'L':
211  gl_map->enableLights(!gl_map->areLightsEnabled());
212  break;
213  };
214  update_msg = true;
215  }
216 
217  if (update_msg)
218  {
219  update_msg = false;
220 
221  win.addTextMessage(
222  5, 5,
223  mrpt::format(
224  "Commands: 'g' (grids=%s) | 'f' (freespace=%s) | 'o' "
225  "(occupied=%s) | 'l' (lights=%s)",
226  gl_map->areGridLinesVisible() ? "YES" : "NO",
227  gl_map->areVoxelsVisible(VOXEL_SET_FREESPACE) ? "YES"
228  : "NO",
229  gl_map->areVoxelsVisible(VOXEL_SET_OCCUPIED) ? "YES" : "NO",
230  gl_map->areLightsEnabled() ? "YES" : "NO"),
231  mrpt::img::TColorf(1, 1, 1), "sans", 15);
232 
233  win.repaint();
234  }
235 
236  std::this_thread::sleep_for(10ms);
237  };
238 }
239 
240 int main(int argc, char** argv)
241 {
242  try
243  {
244  TestOctoMap();
245  return 0;
246  }
247  catch (exception& e)
248  {
249  cout << "MRPT exception caught: " << e.what() << endl;
250  return -1;
251  }
252  catch (...)
253  {
254  printf("Another exception!!");
255  return -1;
256  }
257 }
os.h
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:34
mrpt::obs::CObservation2DRangeScan::loadFromVectors
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
Definition: CObservation2DRangeScan.cpp:555
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:127
COctoMapVoxels.h
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
mrpt::math::TPoint3D::z
double z
Definition: lightweight_geom_data.h:385
COctoMap.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
CDisplayWindow3D.h
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::opengl::COctoMapVoxels::Ptr
std::shared_ptr< COctoMapVoxels > Ptr
Definition: COctoMapVoxels.h:71
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:125
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:385
SCAN_RANGES_1
float SCAN_RANGES_1[]
Definition: vision_stereo_rectify/test.cpp:36
mrpt::img::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:22
mrpt::opengl::VOXEL_SET_OCCUPIED
@ VOXEL_SET_OCCUPIED
Definition: COctoMapVoxels.h:20
mrpt::opengl::VOXEL_SET_FREESPACE
@ VOXEL_SET_FREESPACE
Definition: COctoMapVoxels.h:21
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
mrpt::maps::COctoMap
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMap.h:35
mrpt::math::TPoint3D::y
double y
Definition: lightweight_geom_data.h:385
SCAN_SIZE
#define SCAN_SIZE
Definition: vision_stereo_rectify/test.cpp:32
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< COpenGLScene > Ptr
Definition: COpenGLScene.h:61
M_PIf
#define M_PIf
Definition: common.h:61
SCAN_VALID_1
char SCAN_VALID_1[]
Definition: vision_stereo_rectify/test.cpp:83
mrpt::maps
Definition: CBeacon.h:24
TestOctoMap
void TestOctoMap()
Definition: vision_stereo_rectify/test.cpp:101
CGridPlaneXY.h
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
CObservation2DRangeScan.h


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST