MRPT  2.0.3
COctoMap_unittest.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-2020, 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 <gtest/gtest.h>
11 #include <mrpt/maps/COctoMap.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::maps;
17 using namespace mrpt::obs;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 TEST(COctoMapTests, updateVoxels)
23 {
24  // Copied from the example program in the "octomap" C++ library.
25 
26  COctoMap map(0.1);
27 
28  map.updateVoxel(1, 1, 1, true); // integrate 'occupied' measurement
29 
30  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
31  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
32  map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement
33 
34  map.updateVoxel(-1, -1, 1, false); // integrate 'occupied' measurement
35 
36  double occup;
37  bool is_mapped;
39 
40  pt = mrpt::math::TPoint3D(1, 1, 1);
41  is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
42  EXPECT_GT(occup, 0.5);
43  EXPECT_TRUE(is_mapped);
44 
45  pt = mrpt::math::TPoint3D(-1, -1, 1);
46  is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
47  EXPECT_LT(occup, 0.5);
48  EXPECT_TRUE(is_mapped);
49 }
50 
51 TEST(COctoMapTests, insert2DScan)
52 {
53  // Load scans:
56 
57  // Insert the scan in the map and check expected values:
58  {
59  COctoMap map(0.1);
60  map.insertObservation(scan1);
61  }
62 }
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::math::TPoint3D_< double >
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::maps::CMetricMap::insertObservation
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
COctoMap.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
stock_observations.h
TEST
TEST(COctoMapTests, updateVoxels)
Definition: COctoMap_unittest.cpp:22
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:242
EXPECT_LT
EXPECT_LT(out.final_rmse, 3.0)
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::maps::COctoMap
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMap.h:40
mrpt::obs::stock_observations::example2DRangeScan
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
Definition: stock_observations.cpp:23
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps
Definition: CBeacon.h:21
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:298
EXPECT_GT
EXPECT_GT(out.final_iters, 10UL)
CObservation2DRangeScan.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020