Main MRPT website > C++ reference for MRPT 1.9.9
COctoMap_unittest.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/obs/CObservation2DRangeScan.h>
12 #include <gtest/gtest.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::maps;
16 using namespace mrpt::obs;
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 TEST(COctoMapTests, updateVoxels)
22 {
23  // Copied from the example program in the "octomap" C++ library.
24 
25  COctoMap map(0.1);
26 
27  map.updateVoxel(1, 1, 1, true); // integrate 'occupied' measurement
28 
29  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
30  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
31  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
32 
33  map.updateVoxel(-1, -1, 1, false); // integrate 'occupied' measurement
34 
35  double occup;
36  bool is_mapped;
37  mrpt::math::TPoint3D pt;
38 
39  pt = mrpt::math::TPoint3D(1, 1, 1);
40  is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
41  EXPECT_GT(occup, 0.5);
42  EXPECT_TRUE(is_mapped);
43 
44  pt = mrpt::math::TPoint3D(-1, -1, 1);
45  is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
46  EXPECT_LT(occup, 0.5);
47  EXPECT_TRUE(is_mapped);
48 }
49 
50 TEST(COctoMapTests, insert2DScan)
51 {
52  float SCAN_RANGES_1[] = {
53  0.910f, 0.900f, 0.910f, 0.900f, 0.900f, 0.890f, 0.890f, 0.880f,
54  0.890f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.870f,
55  0.880f, 0.870f, 0.870f, 0.870f, 0.880f, 0.880f, 0.880f, 0.880f,
56  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
57  0.880f, 0.880f, 0.880f, 0.880f, 0.890f, 0.880f, 0.880f, 0.880f,
58  0.890f, 0.880f, 0.890f, 0.890f, 0.880f, 0.890f, 0.890f, 0.880f,
59  0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.900f, 0.900f,
60  0.900f, 0.900f, 0.900f, 0.910f, 0.910f, 0.910f, 0.910f, 0.920f,
61  0.920f, 0.920f, 0.920f, 0.920f, 0.930f, 0.930f, 0.930f, 0.930f,
62  0.940f, 0.940f, 0.950f, 0.950f, 0.950f, 0.950f, 0.960f, 0.960f,
63  0.970f, 0.970f, 0.970f, 0.980f, 0.980f, 0.990f, 1.000f, 1.000f,
64  1.000f, 1.010f, 1.010f, 1.020f, 1.030f, 1.030f, 1.030f, 1.040f,
65  1.050f, 1.060f, 1.050f, 1.060f, 1.070f, 1.070f, 1.080f, 1.080f,
66  1.090f, 1.100f, 1.110f, 1.120f, 1.120f, 1.130f, 1.140f, 1.140f,
67  1.160f, 1.170f, 1.180f, 1.180f, 1.190f, 1.200f, 1.220f, 1.220f,
68  1.230f, 1.230f, 1.240f, 1.250f, 1.270f, 1.280f, 1.290f, 1.300f,
69  1.320f, 1.320f, 1.350f, 1.360f, 1.370f, 1.390f, 1.410f, 1.410f,
70  1.420f, 1.430f, 1.450f, 1.470f, 1.490f, 1.500f, 1.520f, 1.530f,
71  1.560f, 1.580f, 1.600f, 1.620f, 1.650f, 1.670f, 1.700f, 1.730f,
72  1.750f, 1.780f, 1.800f, 1.830f, 1.850f, 1.880f, 1.910f, 1.940f,
73  1.980f, 2.010f, 2.060f, 2.090f, 2.130f, 2.180f, 2.220f, 2.250f,
74  2.300f, 2.350f, 2.410f, 2.460f, 2.520f, 2.570f, 2.640f, 2.700f,
75  2.780f, 2.850f, 2.930f, 3.010f, 3.100f, 3.200f, 3.300f, 3.390f,
76  3.500f, 3.620f, 3.770f, 3.920f, 4.070f, 4.230f, 4.430f, 4.610f,
77  4.820f, 5.040f, 5.290f, 5.520f, 8.970f, 8.960f, 8.950f, 8.930f,
78  8.940f, 8.930f, 9.050f, 9.970f, 9.960f, 10.110f, 13.960f, 18.870f,
79  19.290f, 81.910f, 20.890f, 48.750f, 48.840f, 48.840f, 19.970f, 19.980f,
80  19.990f, 15.410f, 20.010f, 19.740f, 17.650f, 17.400f, 14.360f, 12.860f,
81  11.260f, 11.230f, 8.550f, 8.630f, 9.120f, 9.120f, 8.670f, 8.570f,
82  7.230f, 7.080f, 7.040f, 6.980f, 6.970f, 5.260f, 5.030f, 4.830f,
83  4.620f, 4.440f, 4.390f, 4.410f, 4.410f, 4.410f, 4.430f, 4.440f,
84  4.460f, 4.460f, 4.490f, 4.510f, 4.540f, 3.970f, 3.820f, 3.730f,
85  3.640f, 3.550f, 3.460f, 3.400f, 3.320f, 3.300f, 3.320f, 3.320f,
86  3.340f, 2.790f, 2.640f, 2.600f, 2.570f, 2.540f, 2.530f, 2.510f,
87  2.490f, 2.490f, 2.480f, 2.470f, 2.460f, 2.460f, 2.460f, 2.450f,
88  2.450f, 2.450f, 2.460f, 2.460f, 2.470f, 2.480f, 2.490f, 2.490f,
89  2.520f, 2.510f, 2.550f, 2.570f, 2.610f, 2.640f, 2.980f, 3.040f,
90  3.010f, 2.980f, 2.940f, 2.920f, 2.890f, 2.870f, 2.830f, 2.810f,
91  2.780f, 2.760f, 2.740f, 2.720f, 2.690f, 2.670f, 2.650f, 2.630f,
92  2.620f, 2.610f, 2.590f, 2.560f, 2.550f, 2.530f, 2.510f, 2.500f,
93  2.480f, 2.460f, 2.450f, 2.430f, 2.420f, 2.400f, 2.390f, 2.380f,
94  2.360f, 2.350f, 2.340f, 2.330f, 2.310f, 2.300f, 2.290f, 2.280f,
95  2.270f, 2.260f, 2.250f, 2.240f, 2.230f, 2.230f, 2.220f, 2.210f,
96  2.200f, 2.190f, 2.180f, 2.170f, 1.320f, 1.140f, 1.130f, 1.130f,
97  1.120f, 1.120f, 1.110f, 1.110f, 1.110f, 1.110f, 1.100f, 1.110f,
98  1.100f};
99  char SCAN_VALID_1[] = {
100  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
101  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
102  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
103  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
104  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
105  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
106  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
107  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
108  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
109  1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
110  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
111  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
112  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
113  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
114  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
115  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
116 
117  const size_t SCAN_SIZE = sizeof(SCAN_RANGES_1) / sizeof(SCAN_RANGES_1[0]);
118 
119  // Load scans:
120  mrpt::obs::CObservation2DRangeScan scan1;
121  scan1.aperture = M_PIf;
122  scan1.rightToLeft = true;
123  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCAN_SIZE);
124 
125  scan1.loadFromVectors(SCAN_SIZE, SCAN_RANGES_1, SCAN_VALID_1);
126 
127  // Insert the scan in the map and check expected values:
128  {
129  COctoMap map(0.1);
130  map.insertObservation(&scan1);
131  }
132 }
mrpt::maps::CMetricMap::insertObservation
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
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
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::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
TEST
TEST(COctoMapTests, updateVoxels)
Definition: COctoMap_unittest.cpp:21
mrpt::maps::COctoMapBase::getPointOccupancy
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
Definition: COctoMapBase_impl.h:214
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
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::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
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
mrpt::maps::COctoMap::updateVoxel
void updateVoxel(const double x, const double y, const double z, bool occupied)
Manually updates the occupancy of the voxel at (x,y,z) as being occupied (true) or free (false),...
Definition: COctoMap.cpp:304
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