OctomapConverterTest.cpp
Go to the documentation of this file.
00001 /*
00002  * OctomapConverterTest.cpp
00003  *
00004  *  Created on: May 1, 2017
00005  *      Author: Jeff Delmerico
00006  *       Institute: University of Zürich, Robotics and Perception Group
00007  */
00008 
00009 // Grid map
00010 #include <grid_map_core/GridMap.hpp>
00011 #include <grid_map_octomap/GridMapOctomapConverter.hpp>
00012 
00013 // Octomap
00014 #include <octomap/octomap.h>
00015 #include <octomap/math/Utils.h>
00016 
00017 // Gtest
00018 #include <gtest/gtest.h>
00019 
00020 // Eigen
00021 #include <Eigen/Core>
00022 
00023 using namespace grid_map;
00024 
00025 TEST(OctomapConversion, convertOctomapToGridMap)
00026 {
00027   // Generate Octomap (a 1m sphere centered at (1,1,1))
00028   // Adapted from octomap unit_tests.cpp
00029   octomap::OcTree octomap(0.05);
00030   octomap::Pointcloud* measurement = new octomap::Pointcloud();
00031 
00032   octomap::point3d origin(1.0f, 1.0f, 1.0f);
00033   octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f);
00034 
00035   for (int i = 0; i < 360; i++) {
00036     for (int j = 0; j < 360; j++) {
00037       octomap::point3d p = origin + point_on_surface;
00038       measurement->push_back(p);
00039       point_on_surface.rotate_IP(0, 0, DEG2RAD(1.));
00040     }
00041     point_on_surface.rotate_IP(0, DEG2RAD(1.), 0);
00042   }
00043   octomap.insertPointCloud(*measurement, origin);
00044 
00045   // Convert to grid map.
00046   GridMap gridMap;
00047   bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridMap);
00048   ASSERT_TRUE(res);
00049 
00050   // Check map info.
00051   Length length = gridMap.getLength();
00052   Vector3 octo_length;
00053   octomap.getMetricSize(octo_length.x(), octo_length.y(), octo_length.z());
00054   EXPECT_FLOAT_EQ(octo_length.x(), length.x());
00055   EXPECT_FLOAT_EQ(octo_length.y(), length.y());
00056   EXPECT_FLOAT_EQ(octomap.getResolution(), gridMap.getResolution());
00057   EXPECT_FLOAT_EQ(2.0 + 0.5 * octomap.getResolution(), gridMap.atPosition("elevation", Position(1.0, 1.0)));
00058 }
00059 
00060 TEST(OctomapConversion, convertOctomapToGridMapWithBoundingBox)
00061 {
00062   // Repeat test with bounding box
00063 
00064   // Generate Octomap (a 1m sphere centered at (1,1,1))
00065   // Adapted from Octomap unit_tests.cpp
00066   octomap::OcTree octomap(0.05);
00067   octomap::Pointcloud* measurement = new octomap::Pointcloud();
00068 
00069   octomap::point3d origin(1.0f, 1.0f, 1.0f);
00070   octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f);
00071 
00072   for (int i = 0; i < 360; i++) {
00073     for (int j = 0; j < 360; j++) {
00074       octomap::point3d p = origin + point_on_surface;
00075       measurement->push_back(p);
00076       point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
00077     }
00078     point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
00079   }
00080   octomap.insertPointCloud(*measurement, origin);
00081 
00082   // Convert to grid map.
00083   GridMap gridmap;
00084   grid_map::Position3 minpt(0.5, 0.5, 0.0);
00085   grid_map::Position3 maxpt(2.0, 2.0, 1.0);
00086   bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridmap, &minpt, &maxpt);
00087   ASSERT_TRUE(res);
00088   Length length = gridmap.getLength();
00089   EXPECT_FLOAT_EQ(1.5, length.x());
00090   EXPECT_FLOAT_EQ(1.5, length.y());
00091   EXPECT_FLOAT_EQ(octomap.getResolution(), gridmap.getResolution());
00092   EXPECT_FLOAT_EQ(0.5 * octomap.getResolution(), gridmap.atPosition("elevation", Position(1.0, 1.0)));
00093 }


grid_map_octomap
Author(s): Jeff Delmerico , Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:26