OctomapConverterTest.cpp
Go to the documentation of this file.
1 /*
2  * OctomapConverterTest.cpp
3  *
4  * Created on: May 1, 2017
5  * Author: Jeff Delmerico
6  * Institute: University of Zürich, Robotics and Perception Group
7  */
8 
9 // Grid map
12 
13 // Octomap
14 #include <octomap/octomap.h>
15 #include <octomap/math/Utils.h>
16 
17 // Gtest
18 #include <gtest/gtest.h>
19 
20 // Eigen
21 #include <Eigen/Core>
22 
23 using namespace grid_map;
24 
25 TEST(OctomapConversion, convertOctomapToGridMap)
26 {
27  // Generate Octomap (a 1m sphere centered at (1,1,1))
28  // Adapted from octomap unit_tests.cpp
30  octomap::Pointcloud* measurement = new octomap::Pointcloud();
31 
32  octomap::point3d origin(1.0f, 1.0f, 1.0f);
33  octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f);
34 
35  for (int i = 0; i < 360; i++) {
36  for (int j = 0; j < 360; j++) {
37  octomap::point3d p = origin + point_on_surface;
38  measurement->push_back(p);
39  point_on_surface.rotate_IP(0, 0, DEG2RAD(1.));
40  }
41  point_on_surface.rotate_IP(0, DEG2RAD(1.), 0);
42  }
43  octomap.insertPointCloud(*measurement, origin);
44 
45  // Convert to grid map.
46  GridMap gridMap;
47  bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridMap);
48  ASSERT_TRUE(res);
49 
50  // Check map info.
51  Length length = gridMap.getLength();
52  Vector3 octo_length;
53  octomap.getMetricSize(octo_length.x(), octo_length.y(), octo_length.z());
54  EXPECT_FLOAT_EQ(octo_length.x(), length.x());
55  EXPECT_FLOAT_EQ(octo_length.y(), length.y());
56  EXPECT_FLOAT_EQ(octomap.getResolution(), gridMap.getResolution());
57  EXPECT_FLOAT_EQ(2.0 + 0.5 * octomap.getResolution(), gridMap.atPosition("elevation", Position(1.0, 1.0)));
58 }
59 
60 TEST(OctomapConversion, convertOctomapToGridMapWithBoundingBox)
61 {
62  // Repeat test with bounding box
63 
64  // Generate Octomap (a 1m sphere centered at (1,1,1))
65  // Adapted from Octomap unit_tests.cpp
67  octomap::Pointcloud* measurement = new octomap::Pointcloud();
68 
69  octomap::point3d origin(1.0f, 1.0f, 1.0f);
70  octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f);
71 
72  for (int i = 0; i < 360; i++) {
73  for (int j = 0; j < 360; j++) {
74  octomap::point3d p = origin + point_on_surface;
75  measurement->push_back(p);
76  point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
77  }
78  point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
79  }
80  octomap.insertPointCloud(*measurement, origin);
81 
82  // Convert to grid map.
83  GridMap gridmap;
84  grid_map::Position3 minpt(0.5, 0.5, 0.0);
85  grid_map::Position3 maxpt(2.0, 2.0, 1.0);
86  bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridmap, &minpt, &maxpt);
87  ASSERT_TRUE(res);
88  Length length = gridmap.getLength();
89  EXPECT_FLOAT_EQ(1.5, length.x());
90  EXPECT_FLOAT_EQ(1.5, length.y());
91  EXPECT_FLOAT_EQ(octomap.getResolution(), gridmap.getResolution());
92  EXPECT_FLOAT_EQ(0.5 * octomap.getResolution(), gridmap.atPosition("elevation", Position(1.0, 1.0)));
93 }
const Length & getLength() const
virtual void getMetricSize(double &x, double &y, double &z)
float & atPosition(const std::string &layer, const Position &position)
static bool fromOctomap(const octomap::OcTree &octomap, const std::string &layer, grid_map::GridMap &gridMap, const grid_map::Position3 *minPoint=nullptr, const grid_map::Position3 *maxPoint=nullptr)
void push_back(float x, float y, float z)
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Eigen::Vector3d Position3
#define EXPECT_FLOAT_EQ(a, b)
double getResolution() const
Eigen::Vector2d Position
TEST(OctomapConversion, convertOctomapToGridMap)
Eigen::Vector3d Vector3
#define DEG2RAD(x)
Eigen::Array2d Length


grid_map_octomap
Author(s): Jeff Delmerico , Péter Fankhauser
autogenerated on Wed Apr 1 2020 03:18:21