MRPT  1.9.9
COccupancyGridMap2D_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/COccupancyGridMap2D.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(COccupancyGridMap2DTests, insert2DScan)
22 {
23  float SCAN_RANGES_1[] = {
24  0.910f, 0.900f, 0.910f, 0.900f, 0.900f, 0.890f, 0.890f, 0.880f,
25  0.890f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.870f,
26  0.880f, 0.870f, 0.870f, 0.870f, 0.880f, 0.880f, 0.880f, 0.880f,
27  0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f, 0.880f,
28  0.880f, 0.880f, 0.880f, 0.880f, 0.890f, 0.880f, 0.880f, 0.880f,
29  0.890f, 0.880f, 0.890f, 0.890f, 0.880f, 0.890f, 0.890f, 0.880f,
30  0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.890f, 0.900f, 0.900f,
31  0.900f, 0.900f, 0.900f, 0.910f, 0.910f, 0.910f, 0.910f, 0.920f,
32  0.920f, 0.920f, 0.920f, 0.920f, 0.930f, 0.930f, 0.930f, 0.930f,
33  0.940f, 0.940f, 0.950f, 0.950f, 0.950f, 0.950f, 0.960f, 0.960f,
34  0.970f, 0.970f, 0.970f, 0.980f, 0.980f, 0.990f, 1.000f, 1.000f,
35  1.000f, 1.010f, 1.010f, 1.020f, 1.030f, 1.030f, 1.030f, 1.040f,
36  1.050f, 1.060f, 1.050f, 1.060f, 1.070f, 1.070f, 1.080f, 1.080f,
37  1.090f, 1.100f, 1.110f, 1.120f, 1.120f, 1.130f, 1.140f, 1.140f,
38  1.160f, 1.170f, 1.180f, 1.180f, 1.190f, 1.200f, 1.220f, 1.220f,
39  1.230f, 1.230f, 1.240f, 1.250f, 1.270f, 1.280f, 1.290f, 1.300f,
40  1.320f, 1.320f, 1.350f, 1.360f, 1.370f, 1.390f, 1.410f, 1.410f,
41  1.420f, 1.430f, 1.450f, 1.470f, 1.490f, 1.500f, 1.520f, 1.530f,
42  1.560f, 1.580f, 1.600f, 1.620f, 1.650f, 1.670f, 1.700f, 1.730f,
43  1.750f, 1.780f, 1.800f, 1.830f, 1.850f, 1.880f, 1.910f, 1.940f,
44  1.980f, 2.010f, 2.060f, 2.090f, 2.130f, 2.180f, 2.220f, 2.250f,
45  2.300f, 2.350f, 2.410f, 2.460f, 2.520f, 2.570f, 2.640f, 2.700f,
46  2.780f, 2.850f, 2.930f, 3.010f, 3.100f, 3.200f, 3.300f, 3.390f,
47  3.500f, 3.620f, 3.770f, 3.920f, 4.070f, 4.230f, 4.430f, 4.610f,
48  4.820f, 5.040f, 5.290f, 5.520f, 8.970f, 8.960f, 8.950f, 8.930f,
49  8.940f, 8.930f, 9.050f, 9.970f, 9.960f, 10.110f, 13.960f, 18.870f,
50  19.290f, 81.910f, 20.890f, 48.750f, 48.840f, 48.840f, 19.970f, 19.980f,
51  19.990f, 15.410f, 20.010f, 19.740f, 17.650f, 17.400f, 14.360f, 12.860f,
52  11.260f, 11.230f, 8.550f, 8.630f, 9.120f, 9.120f, 8.670f, 8.570f,
53  7.230f, 7.080f, 7.040f, 6.980f, 6.970f, 5.260f, 5.030f, 4.830f,
54  4.620f, 4.440f, 4.390f, 4.410f, 4.410f, 4.410f, 4.430f, 4.440f,
55  4.460f, 4.460f, 4.490f, 4.510f, 4.540f, 3.970f, 3.820f, 3.730f,
56  3.640f, 3.550f, 3.460f, 3.400f, 3.320f, 3.300f, 3.320f, 3.320f,
57  3.340f, 2.790f, 2.640f, 2.600f, 2.570f, 2.540f, 2.530f, 2.510f,
58  2.490f, 2.490f, 2.480f, 2.470f, 2.460f, 2.460f, 2.460f, 2.450f,
59  2.450f, 2.450f, 2.460f, 2.460f, 2.470f, 2.480f, 2.490f, 2.490f,
60  2.520f, 2.510f, 2.550f, 2.570f, 2.610f, 2.640f, 2.980f, 3.040f,
61  3.010f, 2.980f, 2.940f, 2.920f, 2.890f, 2.870f, 2.830f, 2.810f,
62  2.780f, 2.760f, 2.740f, 2.720f, 2.690f, 2.670f, 2.650f, 2.630f,
63  2.620f, 2.610f, 2.590f, 2.560f, 2.550f, 2.530f, 2.510f, 2.500f,
64  2.480f, 2.460f, 2.450f, 2.430f, 2.420f, 2.400f, 2.390f, 2.380f,
65  2.360f, 2.350f, 2.340f, 2.330f, 2.310f, 2.300f, 2.290f, 2.280f,
66  2.270f, 2.260f, 2.250f, 2.240f, 2.230f, 2.230f, 2.220f, 2.210f,
67  2.200f, 2.190f, 2.180f, 2.170f, 1.320f, 1.140f, 1.130f, 1.130f,
68  1.120f, 1.120f, 1.110f, 1.110f, 1.110f, 1.110f, 1.100f, 1.110f,
69  1.100f};
70  char SCAN_VALID_1[] = {
71  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
72  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
73  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
74  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
75  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
76  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
77  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
78  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
79  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
80  1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
81  1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 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,
83  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,
85  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};
87 
88  const size_t SCAN_SIZE = sizeof(SCAN_RANGES_1) / sizeof(SCAN_RANGES_1[0]);
89 
90  // Load scans:
91  mrpt::obs::CObservation2DRangeScan scan1;
92  scan1.aperture = M_PIf;
93  scan1.rightToLeft = true;
94  ASSERT_(sizeof(SCAN_RANGES_1) == sizeof(float) * SCAN_SIZE);
95 
96  scan1.loadFromVectors(SCAN_SIZE, SCAN_RANGES_1, SCAN_VALID_1);
97 
98  // Insert the scan in the grid map and check expected values:
99  {
100  COccupancyGridMap2D grid(-50.0f, 50.0f, -50.0f, 50.0f, 0.10f);
101  grid.insertObservation(&scan1);
102 
103  EXPECT_GT(grid.getPos(0.5, 0), 0.51f); // A cell in front of the laser
104  // should have a high "freeness"
105  }
106 }
std
STL namespace.
TEST
TEST(COccupancyGridMap2DTests, insert2DScan)
Definition: COccupancyGridMap2D_unittest.cpp:21
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
M_PIf
#define M_PIf
Definition: common.h:61
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
COccupancyGridMap2D.h
CObservation2DRangeScan.h
SCAN_RANGES_1
float SCAN_RANGES_1[]
Definition: vision_stereo_rectify/test.cpp:36
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:60
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:56
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
SCAN_SIZE
#define SCAN_SIZE
Definition: vision_stereo_rectify/test.cpp:32
mrpt::obs::CObservation2DRangeScan::loadFromVectors
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
Definition: CObservation2DRangeScan.cpp:552
SCAN_VALID_1
char SCAN_VALID_1[]
Definition: vision_stereo_rectify/test.cpp:83
mrpt::maps::COccupancyGridMap2D::getPos
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
Definition: COccupancyGridMap2D.h:414
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::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:127


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020