MRPT  1.9.9
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-2019, 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 }
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
EXPECT_LT(out.final_rmse, 3.0)
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
EXPECT_GT(out.final_iters, 10UL)
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
STL namespace.
This base provides a set of functions for maths stuff.
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
This namespace contains representation of robot actions and observations.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
TEST(COctoMapTests, updateVoxels)
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMap.h:40
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 338471620 Mon Feb 17 00:12:39 2020 +0100 at lun feb 17 00:15:10 CET 2020