octree_utils.h

Go to the documentation of this file.

 
#ifndef TESSERACT_GEOMETRY_OCTREE_UTILS_H
#define TESSERACT_GEOMETRY_OCTREE_UTILS_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <octomap/OcTree.h>
#include <memory>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_geometry/impl/octree.h>
 
namespace tesseract_geometry
{
struct PointCloud
{
  struct Point
  {
    Point() = default;
    Point(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
 
    double x;
    double y;
    double z;
  };
 
  std::vector<Point> points;
 
  void addPoint(double x, double y, double z) { points.emplace_back(x, y, z); }
};
 
template <typename PointT>
std::unique_ptr<octomap::OcTree>
createOctree(const PointT& point_cloud, const double resolution, const bool prune, const bool binary = true)
{
  auto ot = std::make_unique<octomap::OcTree>(resolution);
 
  for (auto& point : point_cloud.points)
    ot->updateNode(point.x, point.y, point.z, true, true);
 
  // Per the documentation for overload updateNode above with lazy_eval enabled this must be called after all points
  // are added
  ot->updateInnerOccupancy();
  if (binary)
    ot->toMaxLikelihood();
 
  if (prune)
    tesseract_geometry::Octree::prune(*ot);
 
  return ot;
}
}  // namespace tesseract_geometry
#endif  // TESSERACT_GEOMETRY_OCTREE_UTILS_H