octree.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/binary_object.hpp>
#include <boost/serialization/split_member.hpp>
#include <octomap/octomap.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/utils.h>
#include <tesseract_geometry/impl/octree.h>
 
namespace tesseract_geometry
{
Octree::Octree(std::shared_ptr<const octomap::OcTree> octree, OctreeSubType sub_type, bool pruned, bool binary_octree)
  : Geometry(GeometryType::OCTREE)
  , octree_(std::move(octree))
  , sub_type_(sub_type)
  , pruned_(pruned)
  , binary_octree_(binary_octree)
{
}
 
const std::shared_ptr<const octomap::OcTree>& Octree::getOctree() const { return octree_; }
 
OctreeSubType Octree::getSubType() const { return sub_type_; }
 
bool Octree::getPruned() const { return pruned_; }
 
Geometry::Ptr Octree::clone() const { return std::make_shared<Octree>(octree_, sub_type_); }
 
long Octree::calcNumSubShapes() const
{
  long cnt = 0;
  double occupancy_threshold = octree_->getOccupancyThres();
  for (auto it = octree_->begin(static_cast<unsigned char>(octree_->getTreeDepth())), end = octree_->end(); it != end;
       ++it)
    if (it->getOccupancy() >= occupancy_threshold)
      ++cnt;
 
  return cnt;
}
 
bool Octree::isNodeCollapsible(octomap::OcTree& octree, octomap::OcTreeNode* node)
{
  if (!octree.nodeChildExists(node, 0))
    return false;
 
  double occupancy_threshold = octree.getOccupancyThres();
 
  const octomap::OcTreeNode* firstChild = octree.getNodeChild(node, 0);
  if (octree.nodeHasChildren(firstChild) || firstChild->getOccupancy() < occupancy_threshold)
    return false;
 
  for (unsigned int i = 1; i < 8; i++)
  {
    // comparison via getChild so that casts of derived classes ensure
    // that the right == operator gets called
    if (!octree.nodeChildExists(node, i))
      return false;
 
    if (octree.nodeHasChildren(octree.getNodeChild(node, i)))
      return false;
 
    if (octree.getNodeChild(node, i)->getOccupancy() < occupancy_threshold)
      return false;
  }
 
  return true;
}
 
bool Octree::pruneNode(octomap::OcTree& octree, octomap::OcTreeNode* node)
{
  if (!isNodeCollapsible(octree, node))
    return false;
 
  // set value to children's values (all assumed equal)
  node->copyData(*(octree.getNodeChild(node, 0)));
 
  // delete children (known to be leafs at this point!)
  for (unsigned int i = 0; i < 8; i++)
  {
    octree.deleteNodeChild(node, i);
  }
 
  return true;
}
 
// NOLINTNEXTLINE(misc-no-recursion)
void Octree::pruneRecurs(octomap::OcTree& octree,
                         octomap::OcTreeNode* node,
                         unsigned int depth,
                         unsigned int max_depth,
                         unsigned int& num_pruned)
{
  assert(node);
 
  if (depth < max_depth)
  {
    for (unsigned int i = 0; i < 8; i++)
    {
      if (octree.nodeChildExists(node, i))
      {
        pruneRecurs(octree, octree.getNodeChild(node, i), depth + 1, max_depth, num_pruned);
      }
    }
  }  // end if depth
 
  else
  {
    // max level reached
    if (pruneNode(octree, node))
    {
      num_pruned++;
    }
  }
}
 
void Octree::prune(octomap::OcTree& octree)
{
  if (octree.getRoot() == nullptr)
    return;
 
  for (unsigned int depth = octree.getTreeDepth() - 1; depth > 0; --depth)
  {
    unsigned int num_pruned = 0;
    pruneRecurs(octree, octree.getRoot(), 0, depth, num_pruned);
    if (num_pruned == 0)
      break;
  }
}
 
bool Octree::operator==(const Octree& rhs) const
{
  using namespace tesseract_common;
 
  bool equal = true;
  equal &= Geometry::operator==(rhs);
  equal &= sub_type_ == rhs.sub_type_;
  equal &= pruned_ == rhs.pruned_;
  equal &= resolution_ == rhs.resolution_;
 
  // octree_ == rhs.octree_ looks for exact double equality
  equal &= octree_->getTreeDepth() == rhs.octree_->getTreeDepth();                             // tree_depth
  equal &= almostEqualRelativeAndAbs(octree_->getResolution(), rhs.octree_->getResolution());  // resolution
  equal &= octree_->size() == rhs.octree_->size();                                             // tree_size
  equal &= octree_->getNumLeafNodes() == rhs.octree_->getNumLeafNodes();
  equal &= calcNumSubShapes() == rhs.calcNumSubShapes();
 
  for (auto it = octree_->begin(static_cast<unsigned char>(octree_->getTreeDepth())), end = octree_->end(); it != end;
       ++it)
  {
    const auto coord = it.getCoordinate();
    const auto* node = rhs.octree_->search(coord);
 
    if (node != nullptr)
    {
      equal &= almostEqualRelativeAndAbs(it->getValue(), node->getValue());
      equal &= almostEqualRelativeAndAbs(it->getOccupancy(), node->getOccupancy());
    }
    else
      return false;
  }
 
  return equal;
}
bool Octree::operator!=(const Octree& rhs) const { return !operator==(rhs); }
 
template <class Archive>
void Octree::save(Archive& ar, const unsigned int /*version*/) const
{
  using namespace boost::serialization;
  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Geometry);
  ar& BOOST_SERIALIZATION_NVP(sub_type_);
  ar& BOOST_SERIALIZATION_NVP(resolution_);
  ar& BOOST_SERIALIZATION_NVP(pruned_);
  ar& BOOST_SERIALIZATION_NVP(binary_octree_);
 
  // Read the data to a stream which does not guarantee contiguous memory
  std::ostringstream s;
  if (binary_octree_)
    octree_->writeBinaryConst(s);
  else
    octree_->write(s);
 
  // Write it to a string, wich does guarantee contiguous memory
  std::string data_string = s.str();
  std::size_t octree_data_size = data_string.size();
  ar& make_nvp("octree_data_size", octree_data_size);
  ar& make_nvp("octree_data", make_binary_object(data_string.data(), octree_data_size));
}
 
template <class Archive>
void Octree::load(Archive& ar, const unsigned int /*version*/)
{
  using namespace boost::serialization;
  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Geometry);
  ar& BOOST_SERIALIZATION_NVP(sub_type_);
  ar& BOOST_SERIALIZATION_NVP(resolution_);
  ar& BOOST_SERIALIZATION_NVP(pruned_);
  ar& BOOST_SERIALIZATION_NVP(binary_octree_);
 
  // Initialize the octree to the right size
  auto local_octree = std::make_shared<octomap::OcTree>(resolution_);
 
  // Read the data into a string
  std::size_t octree_data_size = 0;
  ar& make_nvp("octree_data_size", octree_data_size);
  std::string data_string;
  data_string.resize(octree_data_size);
  ar& make_nvp("octree_data", make_binary_object(data_string.data(), octree_data_size));
 
  // Write that data into the stringstream required by octree and load data
  std::stringstream s;
  s.write(data_string.data(), static_cast<std::streamsize>(octree_data_size));
 
  if (binary_octree_)
    local_octree->readBinary(s);
  else
    local_octree = std::shared_ptr<octomap::OcTree>(dynamic_cast<octomap::OcTree*>(octomap::OcTree::read(s)));
 
  octree_ = local_octree;
}
 
template <class Archive>
void Octree::serialize(Archive& ar, const unsigned int version)
{
  boost::serialization::split_member(ar, *this, version);
}
}  // namespace tesseract_geometry
 
#include <tesseract_common/serialization.h>
BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_geometry::Octree)
TESSERACT_SERIALIZE_SAVE_LOAD_ARCHIVES_INSTANTIATE(tesseract_geometry::Octree)