octree.h

Go to the documentation of this file.

 
#ifndef TESSERACT_GEOMETRY_OCTREE_H
#define TESSERACT_GEOMETRY_OCTREE_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/export.hpp>
#include <Eigen/Geometry>
#include <memory>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_geometry/geometry.h>
 
namespace boost::serialization
{
class access;
}
 
namespace octomap
{
class OcTree;
class OcTreeNode;
}  // namespace octomap
 
namespace tesseract_geometry
{
enum class OctreeSubType : std::uint8_t
{
  BOX,
  SPHERE_INSIDE,
  SPHERE_OUTSIDE
};
 
class Octree : public Geometry
{
public:
  using Ptr = std::shared_ptr<Octree>;
  using ConstPtr = std::shared_ptr<const Octree>;
 
  Octree(std::shared_ptr<const octomap::OcTree> octree,
         OctreeSubType sub_type,
         bool pruned = false,
         bool binary_octree = false);
  Octree() = default;
  ~Octree() override = default;
 
  const std::shared_ptr<const octomap::OcTree>& getOctree() const;
 
  OctreeSubType getSubType() const;
 
  bool getPruned() const;
 
  Geometry::Ptr clone() const override final;
 
  bool operator==(const Octree& rhs) const;
  bool operator!=(const Octree& rhs) const;
 
  void update() { assert(false); }  // NOLINT
 
  long calcNumSubShapes() const;
 
private:
  std::shared_ptr<const octomap::OcTree> octree_;
  OctreeSubType sub_type_{ OctreeSubType::BOX };
  double resolution_{ 0.01 };
  bool pruned_{ false };
  bool binary_octree_{ false };
 
  static bool isNodeCollapsible(octomap::OcTree& octree, octomap::OcTreeNode* node);
 
  static bool pruneNode(octomap::OcTree& octree, octomap::OcTreeNode* node);
 
  // NOLINTNEXTLINE(misc-no-recursion)
  static void pruneRecurs(octomap::OcTree& octree,
                          octomap::OcTreeNode* node,
                          unsigned int depth,
                          unsigned int max_depth,
                          unsigned int& num_pruned);
 
  friend class boost::serialization::access;
  friend struct tesseract_common::Serialization;
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const;  // NOLINT
 
  template <class Archive>
  void load(Archive& ar, const unsigned int version);  // NOLINT
 
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);  // NOLINT
 
public:
  static void prune(octomap::OcTree& octree);
};
}  // namespace tesseract_geometry
 
BOOST_CLASS_EXPORT_KEY(tesseract_geometry::Octree)
#endif