GridMapOctomapConverter.cpp
Go to the documentation of this file.
1 /*
2  * GridMapOctomapConverter.cpp
3  *
4  * Created on: May 1, 2017
5  * Author: Jeff Delmerico, Peter Fankhauser
6  * Institute: University of Zürich, Robotics and Perception Group
7  */
8 
10 
11 namespace grid_map {
12 
14 {
15 }
16 
18 {
19 }
20 
22  const std::string& layer,
23  grid_map::GridMap& gridMap,
24  const grid_map::Position3* minPoint,
25  const grid_map::Position3* maxPoint)
26 {
27  if (octomap.getTreeType() != "OcTree") {
28  std::cerr << "Octomap conversion only implemented for standard OcTree type." << std::endl;
29  return false;
30  }
31 
32  // Copy octomap in order to expand any pruned occupied cells and maintain constness of input.
33  octomap::OcTree octomapCopy(octomap);
34 
35  // Iterate through leaf nodes and project occupied cells to elevation map.
36  // On the first pass, expand all occupied cells that are not at maximum depth.
37  unsigned int max_depth = octomapCopy.getTreeDepth();
38  // Adapted from octomap octree2pointcloud.cpp.
39  std::vector<octomap::OcTreeNode*> collapsed_occ_nodes;
40  do {
41  collapsed_occ_nodes.clear();
42  for (octomap::OcTree::iterator it = octomapCopy.begin(); it != octomapCopy.end(); ++it) {
43  if (octomapCopy.isNodeOccupied(*it) && it.getDepth() < max_depth) {
44  collapsed_occ_nodes.push_back(&(*it));
45  }
46  }
47  for (std::vector<octomap::OcTreeNode*>::iterator it = collapsed_occ_nodes.begin();
48  it != collapsed_occ_nodes.end(); ++it) {
49  #if OCTOMAP_VERSION_BEFORE_ROS_KINETIC
50  (*it)->expandNode();
51  #else
52  octomapCopy.expandNode(*it);
53  #endif
54  }
55  // std::cout << "Expanded " << collapsed_occ_nodes.size() << " nodes" << std::endl;
56  } while (collapsed_occ_nodes.size() > 0);
57 
58  // Set up grid map geometry.
59  // TODO Figure out whether to center map.
60  double resolution = octomapCopy.getResolution();
61  grid_map::Position3 minBound;
62  grid_map::Position3 maxBound;
63  octomapCopy.getMetricMin(minBound(0), minBound(1), minBound(2));
64  octomapCopy.getMetricMax(maxBound(0), maxBound(1), maxBound(2));
65 
66  // User can provide coordinate limits to only convert a bounding box.
67  octomap::point3d minBbx(minBound(0), minBound(1), minBound(2));
68  if (minPoint) {
69  minBbx = octomap::point3d((*minPoint)(0), (*minPoint)(1), (*minPoint)(2));
70  minBound = grid_map::Position3(minBbx.x(), minBbx.y(), minBbx.z());
71  }
72  octomap::point3d maxBbx(maxBound(0), maxBound(1), maxBound(2));
73  if (maxPoint) {
74  maxBbx = octomap::point3d((*maxPoint)(0), (*maxPoint)(1), (*maxPoint)(2));
75  maxBound = grid_map::Position3(maxBbx.x(), maxBbx.y(), maxBbx.z());
76  }
77 
78  grid_map::Length length = grid_map::Length(maxBound(0) - minBound(0), maxBound(1) - minBound(1));
79  grid_map::Position position = grid_map::Position((maxBound(0) + minBound(0)) / 2.0,
80  (maxBound(1) + minBound(1)) / 2.0);
81  gridMap.setGeometry(length, resolution, position);
82  // std::cout << "grid map geometry: " << std::endl;
83  // std::cout << "Length: [" << length(0) << ", " << length(1) << "]" << std::endl;
84  // std::cout << "Position: [" << position(0) << ", " << position(1) << "]" << std::endl;
85  // std::cout << "Resolution: " << resolution << std::endl;
86 
87  // Add elevation layer
88  gridMap.add(layer);
89  gridMap.setBasicLayers({layer});
90 
91  // For each voxel, if its elevation is higher than the existing value for the
92  // corresponding grid map cell, overwrite it.
93  // std::cout << "Iterating from " << min_bbx << " to " << max_bbx << std::endl;
94  grid_map::Matrix& gridMapData = gridMap[layer];
95  for(octomap::OcTree::leaf_bbx_iterator it = octomapCopy.begin_leafs_bbx(minBbx, maxBbx),
96  end = octomapCopy.end_leafs_bbx(); it != end; ++it) {
97  if (octomapCopy.isNodeOccupied(*it)) {
98  octomap::point3d octoPos = it.getCoordinate();
99  grid_map::Position position(octoPos.x(), octoPos.y());
100  grid_map::Index index;
101  gridMap.getIndex(position, index);
102  // If no elevation has been set, use current elevation.
103  if (!gridMap.isValid(index)) {
104  gridMapData(index(0), index(1)) = octoPos.z();
105  }
106  // Check existing elevation, keep higher.
107  else {
108  if (gridMapData(index(0), index(1)) < octoPos.z()) {
109  gridMapData(index(0), index(1)) = octoPos.z();
110  }
111  }
112  }
113  }
114 
115  return true;
116 }
117 
118 } /* namespace */
Eigen::Array2i Index
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
iterator begin(unsigned char maxDepth=0) const
void setBasicLayers(const std::vector< std::string > &basicLayers)
std::string getTreeType() const
Eigen::MatrixXf Matrix
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)
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
bool isValid(const Index &index) const
Eigen::Vector3d Position3
Eigen::Vector2d Position
virtual void getMetricMin(double &x, double &y, double &z)
virtual void getMetricMax(double &x, double &y, double &z)
void add(const std::string &layer, const double value=NAN)
octomath::Vector3 point3d
bool getIndex(const Position &position, Index &index) const
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
Eigen::Array2d Length


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