1004 lines
33 KiB
C++
1004 lines
33 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2012, Willow Garage, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of Willow Garage, Inc. nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* $Id$
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/octree/octree_container.h>
|
|
#include <pcl/octree/octree_iterator.h>
|
|
#include <pcl/octree/octree_key.h>
|
|
#include <pcl/octree/octree_nodes.h>
|
|
#include <pcl/pcl_macros.h>
|
|
|
|
#include <vector>
|
|
|
|
namespace pcl {
|
|
namespace octree {
|
|
|
|
template <typename ContainerT>
|
|
class BufferedBranchNode : public OctreeNode {
|
|
|
|
public:
|
|
/** \brief Empty constructor. */
|
|
BufferedBranchNode() : OctreeNode() { reset(); }
|
|
|
|
/** \brief Copy constructor. */
|
|
BufferedBranchNode(const BufferedBranchNode& source) : OctreeNode()
|
|
{
|
|
*this = source;
|
|
}
|
|
|
|
/** \brief Copy operator. */
|
|
inline BufferedBranchNode&
|
|
operator=(const BufferedBranchNode& source_arg)
|
|
{
|
|
memset(child_node_array_, 0, sizeof(child_node_array_));
|
|
|
|
for (unsigned char b = 0; b < 2; ++b)
|
|
for (unsigned char i = 0; i < 8; ++i)
|
|
if (source_arg.child_node_array_[b][i])
|
|
child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy();
|
|
|
|
return (*this);
|
|
}
|
|
|
|
/** \brief Empty constructor. */
|
|
~BufferedBranchNode() {}
|
|
|
|
/** \brief Method to perform a deep copy of the octree */
|
|
BufferedBranchNode*
|
|
deepCopy() const override
|
|
{
|
|
return new BufferedBranchNode(*this);
|
|
}
|
|
|
|
/** \brief Get child pointer in current branch node
|
|
* \param buffer_arg: buffer selector
|
|
* \param index_arg: index of child in node
|
|
* \return pointer to child node
|
|
*/
|
|
inline OctreeNode*
|
|
getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
|
|
{
|
|
assert((buffer_arg < 2) && (index_arg < 8));
|
|
return child_node_array_[buffer_arg][index_arg];
|
|
}
|
|
|
|
/** \brief Set child pointer in current branch node
|
|
* \param buffer_arg: buffer selector
|
|
* \param index_arg: index of child in node
|
|
* \param newNode_arg: pointer to new child node
|
|
*/
|
|
inline void
|
|
setChildPtr(unsigned char buffer_arg,
|
|
unsigned char index_arg,
|
|
OctreeNode* newNode_arg)
|
|
{
|
|
assert((buffer_arg < 2) && (index_arg < 8));
|
|
child_node_array_[buffer_arg][index_arg] = newNode_arg;
|
|
}
|
|
|
|
/** \brief Check if branch is pointing to a particular child node
|
|
* \param buffer_arg: buffer selector
|
|
* \param index_arg: index of child in node
|
|
* \return "true" if pointer to child node exists; "false" otherwise
|
|
*/
|
|
inline bool
|
|
hasChild(unsigned char buffer_arg, unsigned char index_arg) const
|
|
{
|
|
assert((buffer_arg < 2) && (index_arg < 8));
|
|
return (child_node_array_[buffer_arg][index_arg] != nullptr);
|
|
}
|
|
|
|
/** \brief Get the type of octree node. Returns LEAVE_NODE type */
|
|
node_type_t
|
|
getNodeType() const override
|
|
{
|
|
return BRANCH_NODE;
|
|
}
|
|
|
|
/** \brief Reset branch node container for every branch buffer. */
|
|
inline void
|
|
reset()
|
|
{
|
|
memset(&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
|
|
}
|
|
|
|
/** \brief Get const pointer to container */
|
|
const ContainerT*
|
|
operator->() const
|
|
{
|
|
return &container_;
|
|
}
|
|
|
|
/** \brief Get pointer to container */
|
|
ContainerT*
|
|
operator->()
|
|
{
|
|
return &container_;
|
|
}
|
|
|
|
/** \brief Get const reference to container */
|
|
const ContainerT&
|
|
operator*() const
|
|
{
|
|
return container_;
|
|
}
|
|
|
|
/** \brief Get reference to container */
|
|
ContainerT&
|
|
operator*()
|
|
{
|
|
return container_;
|
|
}
|
|
|
|
/** \brief Get const reference to container */
|
|
const ContainerT&
|
|
getContainer() const
|
|
{
|
|
return container_;
|
|
}
|
|
|
|
/** \brief Get reference to container */
|
|
ContainerT&
|
|
getContainer()
|
|
{
|
|
return container_;
|
|
}
|
|
|
|
/** \brief Get const pointer to container */
|
|
const ContainerT*
|
|
getContainerPtr() const
|
|
{
|
|
return &container_;
|
|
}
|
|
|
|
/** \brief Get pointer to container */
|
|
ContainerT*
|
|
getContainerPtr()
|
|
{
|
|
return &container_;
|
|
}
|
|
|
|
protected:
|
|
ContainerT container_;
|
|
|
|
OctreeNode* child_node_array_[2][8];
|
|
};
|
|
|
|
/** \brief @b Octree double buffer class
|
|
*
|
|
* This octree implementation keeps two separate octree structures in memory
|
|
* which allows for differentially comparison of the octree structures (change
|
|
* detection, differential encoding).
|
|
* \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should
|
|
* be initially defined).
|
|
* \note All leaf nodes are addressed by integer indices.
|
|
* \note The tree depth equates to the bit length of the voxel indices.
|
|
* \ingroup octree
|
|
* \author Julius Kammerl (julius@kammerl.de)
|
|
*/
|
|
template <typename LeafContainerT = index_t,
|
|
typename BranchContainerT = OctreeContainerEmpty>
|
|
class Octree2BufBase {
|
|
|
|
public:
|
|
using OctreeT = Octree2BufBase<LeafContainerT, BranchContainerT>;
|
|
|
|
// iterators are friends
|
|
friend class OctreeIteratorBase<OctreeT>;
|
|
friend class OctreeDepthFirstIterator<OctreeT>;
|
|
friend class OctreeBreadthFirstIterator<OctreeT>;
|
|
friend class OctreeLeafNodeDepthFirstIterator<OctreeT>;
|
|
friend class OctreeLeafNodeBreadthFirstIterator<OctreeT>;
|
|
|
|
using BranchNode = BufferedBranchNode<BranchContainerT>;
|
|
using LeafNode = OctreeLeafNode<LeafContainerT>;
|
|
|
|
using BranchContainer = BranchContainerT;
|
|
using LeafContainer = LeafContainerT;
|
|
|
|
// Octree default iterators
|
|
using Iterator = OctreeDepthFirstIterator<OctreeT>;
|
|
using ConstIterator = const OctreeDepthFirstIterator<OctreeT>;
|
|
Iterator
|
|
begin(uindex_t max_depth_arg = 0)
|
|
{
|
|
return Iterator(this, max_depth_arg);
|
|
};
|
|
const Iterator
|
|
end()
|
|
{
|
|
return Iterator();
|
|
};
|
|
|
|
// Octree leaf node iterators
|
|
// The previous deprecated names
|
|
// LeafNodeIterator and ConstLeafNodeIterator are deprecated.
|
|
// Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
|
|
using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
|
|
using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator<OctreeT>;
|
|
|
|
// The currently valide names
|
|
using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator<OctreeT>;
|
|
using ConstLeafNodeDepthFirstIterator =
|
|
const OctreeLeafNodeDepthFirstIterator<OctreeT>;
|
|
LeafNodeDepthFirstIterator
|
|
leaf_depth_begin(uindex_t max_depth_arg = 0)
|
|
{
|
|
return LeafNodeDepthFirstIterator(this, max_depth_arg);
|
|
};
|
|
|
|
const LeafNodeDepthFirstIterator
|
|
leaf_depth_end()
|
|
{
|
|
return LeafNodeDepthFirstIterator();
|
|
};
|
|
|
|
// Octree depth-first iterators
|
|
using DepthFirstIterator = OctreeDepthFirstIterator<OctreeT>;
|
|
using ConstDepthFirstIterator = const OctreeDepthFirstIterator<OctreeT>;
|
|
DepthFirstIterator
|
|
depth_begin(uindex_t maxDepth_arg = 0)
|
|
{
|
|
return DepthFirstIterator(this, maxDepth_arg);
|
|
};
|
|
const DepthFirstIterator
|
|
depth_end()
|
|
{
|
|
return DepthFirstIterator();
|
|
};
|
|
|
|
// Octree breadth-first iterators
|
|
using BreadthFirstIterator = OctreeBreadthFirstIterator<OctreeT>;
|
|
using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator<OctreeT>;
|
|
BreadthFirstIterator
|
|
breadth_begin(uindex_t max_depth_arg = 0)
|
|
{
|
|
return BreadthFirstIterator(this, max_depth_arg);
|
|
};
|
|
const BreadthFirstIterator
|
|
breadth_end()
|
|
{
|
|
return BreadthFirstIterator();
|
|
};
|
|
|
|
// Octree leaf node iterators
|
|
using LeafNodeBreadthIterator = OctreeLeafNodeBreadthFirstIterator<OctreeT>;
|
|
using ConstLeafNodeBreadthIterator =
|
|
const OctreeLeafNodeBreadthFirstIterator<OctreeT>;
|
|
|
|
LeafNodeBreadthIterator
|
|
leaf_breadth_begin(uindex_t max_depth_arg = 0u)
|
|
{
|
|
return LeafNodeBreadthIterator(this,
|
|
max_depth_arg ? max_depth_arg : this->octree_depth_);
|
|
};
|
|
|
|
const LeafNodeBreadthIterator
|
|
leaf_breadth_end()
|
|
{
|
|
return LeafNodeBreadthIterator(this, 0, nullptr);
|
|
};
|
|
|
|
/** \brief Empty constructor. */
|
|
Octree2BufBase();
|
|
|
|
/** \brief Empty deconstructor. */
|
|
virtual ~Octree2BufBase();
|
|
|
|
/** \brief Copy constructor. */
|
|
Octree2BufBase(const Octree2BufBase& source)
|
|
: leaf_count_(source.leaf_count_)
|
|
, branch_count_(source.branch_count_)
|
|
, root_node_(new (BranchNode)(*(source.root_node_)))
|
|
, depth_mask_(source.depth_mask_)
|
|
, max_key_(source.max_key_)
|
|
, buffer_selector_(source.buffer_selector_)
|
|
, tree_dirty_flag_(source.tree_dirty_flag_)
|
|
, octree_depth_(source.octree_depth_)
|
|
, dynamic_depth_enabled_(source.dynamic_depth_enabled_)
|
|
{}
|
|
|
|
/** \brief Copy constructor. */
|
|
inline Octree2BufBase&
|
|
operator=(const Octree2BufBase& source)
|
|
{
|
|
leaf_count_ = source.leaf_count_;
|
|
branch_count_ = source.branch_count_;
|
|
root_node_ = new (BranchNode)(*(source.root_node_));
|
|
depth_mask_ = source.depth_mask_;
|
|
max_key_ = source.max_key_;
|
|
buffer_selector_ = source.buffer_selector_;
|
|
tree_dirty_flag_ = source.tree_dirty_flag_;
|
|
octree_depth_ = source.octree_depth_;
|
|
dynamic_depth_enabled_ = source.dynamic_depth_enabled_;
|
|
return (*this);
|
|
}
|
|
|
|
/** \brief Set the maximum amount of voxels per dimension.
|
|
* \param max_voxel_index_arg: maximum amount of voxels per dimension
|
|
*/
|
|
void
|
|
setMaxVoxelIndex(uindex_t max_voxel_index_arg);
|
|
|
|
/** \brief Set the maximum depth of the octree.
|
|
* \param depth_arg: maximum depth of octree
|
|
*/
|
|
void
|
|
setTreeDepth(uindex_t depth_arg);
|
|
|
|
/** \brief Get the maximum depth of the octree.
|
|
* \return depth_arg: maximum depth of octree
|
|
*/
|
|
inline uindex_t
|
|
getTreeDepth() const
|
|
{
|
|
return this->octree_depth_;
|
|
}
|
|
|
|
/** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
|
|
* \note If leaf node already exist, this method returns the existing node
|
|
* \param idx_x_arg: index of leaf node in the X axis.
|
|
* \param idx_y_arg: index of leaf node in the Y axis.
|
|
* \param idx_z_arg: index of leaf node in the Z axis.
|
|
* \return pointer to new leaf node container.
|
|
*/
|
|
LeafContainerT*
|
|
createLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
|
|
|
|
/** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
|
|
* \note If leaf node already exist, this method returns the existing node
|
|
* \param idx_x_arg: index of leaf node in the X axis.
|
|
* \param idx_y_arg: index of leaf node in the Y axis.
|
|
* \param idx_z_arg: index of leaf node in the Z axis.
|
|
* \return pointer to leaf node container if found, null pointer otherwise.
|
|
*/
|
|
LeafContainerT*
|
|
findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
|
|
|
|
/** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
|
|
* \param idx_x_arg: index of leaf node in the X axis.
|
|
* \param idx_y_arg: index of leaf node in the Y axis.
|
|
* \param idx_z_arg: index of leaf node in the Z axis.
|
|
* \return "true" if leaf node search is successful, otherwise it returns "false".
|
|
*/
|
|
bool
|
|
existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const;
|
|
|
|
/** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
|
|
* \param idx_x_arg: index of leaf node in the X axis.
|
|
* \param idx_y_arg: index of leaf node in the Y axis.
|
|
* \param idx_z_arg: index of leaf node in the Z axis.
|
|
*/
|
|
void
|
|
removeLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
|
|
|
|
/** \brief Return the amount of existing leafs in the octree.
|
|
* \return amount of registered leaf nodes.
|
|
*/
|
|
inline std::size_t
|
|
getLeafCount() const
|
|
{
|
|
return (leaf_count_);
|
|
}
|
|
|
|
/** \brief Return the amount of existing branches in the octree.
|
|
* \return amount of branch nodes.
|
|
*/
|
|
inline std::size_t
|
|
getBranchCount() const
|
|
{
|
|
return (branch_count_);
|
|
}
|
|
|
|
/** \brief Delete the octree structure and its leaf nodes.
|
|
*/
|
|
void
|
|
deleteTree();
|
|
|
|
/** \brief Delete octree structure of previous buffer. */
|
|
inline void
|
|
deletePreviousBuffer()
|
|
{
|
|
treeCleanUpRecursive(root_node_);
|
|
}
|
|
|
|
/** \brief Delete the octree structure in the current buffer. */
|
|
inline void
|
|
deleteCurrentBuffer()
|
|
{
|
|
buffer_selector_ = !buffer_selector_;
|
|
treeCleanUpRecursive(root_node_);
|
|
leaf_count_ = 0;
|
|
}
|
|
|
|
/** \brief Switch buffers and reset current octree structure. */
|
|
void
|
|
switchBuffers();
|
|
|
|
/** \brief Serialize octree into a binary output vector describing its branch node
|
|
* structure.
|
|
* \param binary_tree_out_arg: reference to output vector for writing binary
|
|
* tree structure.
|
|
* \param do_XOR_encoding_arg: select if binary tree structure should be generated
|
|
* based on current octree (false) of based on a XOR comparison between current and
|
|
* previous octree
|
|
**/
|
|
void
|
|
serializeTree(std::vector<char>& binary_tree_out_arg,
|
|
bool do_XOR_encoding_arg = false);
|
|
|
|
/** \brief Serialize octree into a binary output vector describing its branch node
|
|
* structure and and push all DataT elements stored in the octree to a vector.
|
|
* \param binary_tree_out_arg: reference to output vector for writing binary tree
|
|
* structure.
|
|
* \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the
|
|
* octree
|
|
* \param do_XOR_encoding_arg: select if binary tree structure should be
|
|
* generated based on current octree (false) of based on a XOR comparison between
|
|
* current and previous octree
|
|
**/
|
|
void
|
|
serializeTree(std::vector<char>& binary_tree_out_arg,
|
|
std::vector<LeafContainerT*>& leaf_container_vector_arg,
|
|
bool do_XOR_encoding_arg = false);
|
|
|
|
/** \brief Outputs a vector of all DataT elements that are stored within the octree
|
|
* leaf nodes.
|
|
* \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
|
|
* in the octree
|
|
*/
|
|
void
|
|
serializeLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
|
|
|
|
/** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist
|
|
* in the previous octree buffer.
|
|
* \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
|
|
* in the octree
|
|
*/
|
|
void
|
|
serializeNewLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
|
|
|
|
/** \brief Deserialize a binary octree description vector and create a corresponding
|
|
* octree structure. Leaf nodes are initialized with getDataTByKey(..).
|
|
* \param binary_tree_in_arg: reference to input vector for reading binary tree
|
|
* structure.
|
|
* \param do_XOR_decoding_arg: select if binary tree structure is based on current
|
|
* octree (false) of based on a XOR comparison between current and previous octree
|
|
*/
|
|
void
|
|
deserializeTree(std::vector<char>& binary_tree_in_arg,
|
|
bool do_XOR_decoding_arg = false);
|
|
|
|
/** \brief Deserialize a binary octree description and create a corresponding octree
|
|
* structure. Leaf nodes are initialized with DataT elements from the dataVector.
|
|
* \param binary_tree_in_arg: reference to inpvectoream for reading binary tree
|
|
* structure.
|
|
* \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
|
|
* in the octree
|
|
* \param do_XOR_decoding_arg: select if binary tree structure is based on current
|
|
* octree (false) of based on a XOR comparison between current and previous octree
|
|
*/
|
|
void
|
|
deserializeTree(std::vector<char>& binary_tree_in_arg,
|
|
std::vector<LeafContainerT*>& leaf_container_vector_arg,
|
|
bool do_XOR_decoding_arg = false);
|
|
|
|
protected:
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Protected octree methods based on octree keys
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/** \brief Retrieve root node */
|
|
OctreeNode*
|
|
getRootNode() const
|
|
{
|
|
return (this->root_node_);
|
|
}
|
|
|
|
/** \brief Find leaf node
|
|
* \param key_arg: octree key addressing a leaf node.
|
|
* \return pointer to leaf container. If leaf node is not found, this pointer returns
|
|
* 0.
|
|
*/
|
|
inline LeafContainerT*
|
|
findLeaf(const OctreeKey& key_arg) const
|
|
{
|
|
LeafContainerT* result = nullptr;
|
|
findLeafRecursive(key_arg, depth_mask_, root_node_, result);
|
|
return result;
|
|
}
|
|
|
|
/** \brief Create a leaf node.
|
|
* \note If the leaf node at the given octree node does not exist, it will be created
|
|
* and added to the tree.
|
|
* \param key_arg: octree key addressing a leaf node.
|
|
* \return pointer to an existing or created leaf container.
|
|
*/
|
|
inline LeafContainerT*
|
|
createLeaf(const OctreeKey& key_arg)
|
|
{
|
|
LeafNode* leaf_node;
|
|
BranchNode* leaf_node_parent;
|
|
|
|
createLeafRecursive(
|
|
key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent, false);
|
|
|
|
LeafContainerT* ret = leaf_node->getContainerPtr();
|
|
|
|
return ret;
|
|
}
|
|
|
|
/** \brief Check if leaf doesn't exist in the octree
|
|
* \param key_arg: octree key addressing a leaf node.
|
|
* \return "true" if leaf node is found; "false" otherwise
|
|
*/
|
|
inline bool
|
|
existLeaf(const OctreeKey& key_arg) const
|
|
{
|
|
return (findLeaf(key_arg) != nullptr);
|
|
}
|
|
|
|
/** \brief Remove leaf node from octree
|
|
* \param key_arg: octree key addressing a leaf node.
|
|
*/
|
|
inline void
|
|
removeLeaf(const OctreeKey& key_arg)
|
|
{
|
|
if (key_arg <= max_key_) {
|
|
deleteLeafRecursive(key_arg, depth_mask_, root_node_);
|
|
|
|
// we changed the octree structure -> dirty
|
|
tree_dirty_flag_ = true;
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Branch node accessor inline functions
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/** \brief Check if branch is pointing to a particular child node
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param child_idx_arg: index to child node
|
|
* \return "true" if pointer to child node exists; "false" otherwise
|
|
*/
|
|
inline bool
|
|
branchHasChild(const BranchNode& branch_arg, unsigned char child_idx_arg) const
|
|
{
|
|
// test occupancyByte for child existence
|
|
return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr);
|
|
}
|
|
|
|
/** \brief Retrieve a child node pointer for child node at child_idx.
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param child_idx_arg: index to child node
|
|
* \return pointer to octree child node class
|
|
*/
|
|
inline OctreeNode*
|
|
getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const
|
|
{
|
|
return branch_arg.getChildPtr(buffer_selector_, child_idx_arg);
|
|
}
|
|
|
|
/** \brief Assign new child node to branch
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param child_idx_arg: index to child node
|
|
* \param new_child_arg: pointer to new child node
|
|
*/
|
|
inline void
|
|
setBranchChildPtr(BranchNode& branch_arg,
|
|
unsigned char child_idx_arg,
|
|
OctreeNode* new_child_arg)
|
|
{
|
|
branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_child_arg);
|
|
}
|
|
|
|
/** \brief Generate bit pattern reflecting the existence of child node pointers for
|
|
* current buffer
|
|
* \param branch_arg: reference to octree branch class
|
|
* \return a single byte with 8 bits of child node information
|
|
*/
|
|
inline char
|
|
getBranchBitPattern(const BranchNode& branch_arg) const
|
|
{
|
|
char node_bits;
|
|
|
|
// create bit pattern
|
|
node_bits = 0;
|
|
for (unsigned char i = 0; i < 8; i++) {
|
|
const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i);
|
|
node_bits |= static_cast<char>((!!child) << i);
|
|
}
|
|
|
|
return (node_bits);
|
|
}
|
|
|
|
/** \brief Generate bit pattern reflecting the existence of child node pointers in
|
|
* specific buffer
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param bufferSelector_arg: buffer selector
|
|
* \return a single byte with 8 bits of child node information
|
|
*/
|
|
inline char
|
|
getBranchBitPattern(const BranchNode& branch_arg,
|
|
unsigned char bufferSelector_arg) const
|
|
{
|
|
char node_bits;
|
|
|
|
// create bit pattern
|
|
node_bits = 0;
|
|
for (unsigned char i = 0; i < 8; i++) {
|
|
const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i);
|
|
node_bits |= static_cast<char>((!!child) << i);
|
|
}
|
|
|
|
return (node_bits);
|
|
}
|
|
|
|
/** \brief Generate XOR bit pattern reflecting differences between the two octree
|
|
* buffers
|
|
* \param branch_arg: reference to octree branch class
|
|
* \return a single byte with 8 bits of child node XOR difference information
|
|
*/
|
|
inline char
|
|
getBranchXORBitPattern(const BranchNode& branch_arg) const
|
|
{
|
|
char node_bits[2];
|
|
|
|
// create bit pattern for both buffers
|
|
node_bits[0] = node_bits[1] = 0;
|
|
|
|
for (unsigned char i = 0; i < 8; i++) {
|
|
const OctreeNode* childA = branch_arg.getChildPtr(0, i);
|
|
const OctreeNode* childB = branch_arg.getChildPtr(1, i);
|
|
|
|
node_bits[0] |= static_cast<char>((!!childA) << i);
|
|
node_bits[1] |= static_cast<char>((!!childB) << i);
|
|
}
|
|
|
|
return node_bits[0] ^ node_bits[1];
|
|
}
|
|
|
|
/** \brief Test if branch changed between previous and current buffer
|
|
* \param branch_arg: reference to octree branch class
|
|
* \return "true", if child node information differs between current and previous
|
|
* octree buffer
|
|
*/
|
|
inline bool
|
|
hasBranchChanges(const BranchNode& branch_arg) const
|
|
{
|
|
return (getBranchXORBitPattern(branch_arg) > 0);
|
|
}
|
|
|
|
/** \brief Delete child node and all its subchilds from octree in specific buffer
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param buffer_selector_arg: buffer selector
|
|
* \param child_idx_arg: index to child node
|
|
*/
|
|
inline void
|
|
deleteBranchChild(BranchNode& branch_arg,
|
|
unsigned char buffer_selector_arg,
|
|
unsigned char child_idx_arg)
|
|
{
|
|
if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) {
|
|
OctreeNode* branchChild =
|
|
branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);
|
|
|
|
switch (branchChild->getNodeType()) {
|
|
case BRANCH_NODE: {
|
|
// free child branch recursively
|
|
deleteBranch(*static_cast<BranchNode*>(branchChild));
|
|
|
|
// delete unused branch
|
|
delete (branchChild);
|
|
break;
|
|
}
|
|
|
|
case LEAF_NODE: {
|
|
// push unused leaf to branch pool
|
|
delete (branchChild);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// set branch child pointer to 0
|
|
branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr);
|
|
}
|
|
}
|
|
|
|
/** \brief Delete child node and all its subchilds from octree in current buffer
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param child_idx_arg: index to child node
|
|
*/
|
|
inline void
|
|
deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
|
|
{
|
|
deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
|
|
}
|
|
|
|
/** \brief Delete branch and all its subchilds from octree (both buffers)
|
|
* \param branch_arg: reference to octree branch class
|
|
*/
|
|
inline void
|
|
deleteBranch(BranchNode& branch_arg)
|
|
{
|
|
// delete all branch node children
|
|
for (char i = 0; i < 8; i++) {
|
|
|
|
if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) {
|
|
// reference was copied - there is only one child instance to be deleted
|
|
deleteBranchChild(branch_arg, 0, i);
|
|
|
|
// remove pointers from both buffers
|
|
branch_arg.setChildPtr(0, i, nullptr);
|
|
branch_arg.setChildPtr(1, i, nullptr);
|
|
}
|
|
else {
|
|
deleteBranchChild(branch_arg, 0, i);
|
|
deleteBranchChild(branch_arg, 1, i);
|
|
}
|
|
}
|
|
}
|
|
|
|
/** \brief Fetch and add a new branch child to a branch class in current buffer
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param child_idx_arg: index to child node
|
|
* \return pointer of new branch child to this reference
|
|
*/
|
|
inline BranchNode*
|
|
createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
|
|
{
|
|
BranchNode* new_branch_child = new BranchNode();
|
|
|
|
branch_arg.setChildPtr(
|
|
buffer_selector_, child_idx_arg, static_cast<OctreeNode*>(new_branch_child));
|
|
|
|
return new_branch_child;
|
|
}
|
|
|
|
/** \brief Fetch and add a new leaf child to a branch class
|
|
* \param branch_arg: reference to octree branch class
|
|
* \param child_idx_arg: index to child node
|
|
* \return pointer of new leaf child to this reference
|
|
*/
|
|
inline LeafNode*
|
|
createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg)
|
|
{
|
|
LeafNode* new_leaf_child = new LeafNode();
|
|
|
|
branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);
|
|
|
|
return new_leaf_child;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Recursive octree methods
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/** \brief Create a leaf node at octree key. If leaf node does already exist, it is
|
|
* returned.
|
|
* \param key_arg: reference to an octree key
|
|
* \param depth_mask_arg: depth mask used for octree key analysis and for branch depth
|
|
* indicator
|
|
* \param branch_arg: current branch node
|
|
* \param return_leaf_arg: return pointer to leaf container
|
|
* \param parent_of_leaf_arg: return pointer to parent of leaf node
|
|
* \param branch_reset_arg: Reset pointer array of current branch
|
|
* \return depth mask at which leaf node was created/found
|
|
**/
|
|
uindex_t
|
|
createLeafRecursive(const OctreeKey& key_arg,
|
|
uindex_t depth_mask_arg,
|
|
BranchNode* branch_arg,
|
|
LeafNode*& return_leaf_arg,
|
|
BranchNode*& parent_of_leaf_arg,
|
|
bool branch_reset_arg = false);
|
|
|
|
/** \brief Recursively search for a given leaf node and return a pointer.
|
|
* \note If leaf node does not exist, a 0 pointer is returned.
|
|
* \param key_arg: reference to an octree key
|
|
* \param depth_mask_arg: depth mask used for octree key analysis and for branch
|
|
* depth indicator
|
|
* \param branch_arg: current branch node
|
|
* \param result_arg: pointer to leaf container class
|
|
**/
|
|
void
|
|
findLeafRecursive(const OctreeKey& key_arg,
|
|
uindex_t depth_mask_arg,
|
|
BranchNode* branch_arg,
|
|
LeafContainerT*& result_arg) const;
|
|
|
|
/** \brief Recursively search and delete leaf node
|
|
* \param key_arg: reference to an octree key
|
|
* \param depth_mask_arg: depth mask used for octree key analysis and branch depth
|
|
* indicator
|
|
* \param branch_arg: current branch node
|
|
* \return "true" if branch does not contain any childs; "false" otherwise. This
|
|
* indicates if current branch can be deleted.
|
|
**/
|
|
bool
|
|
deleteLeafRecursive(const OctreeKey& key_arg,
|
|
uindex_t depth_mask_arg,
|
|
BranchNode* branch_arg);
|
|
|
|
/** \brief Recursively explore the octree and output binary octree description
|
|
* together with a vector of leaf node DataT content.
|
|
* \param branch_arg: current branch node
|
|
* \param key_arg: reference to an octree key
|
|
* \param binary_tree_out_arg: binary output vector
|
|
* \param leaf_container_vector_arg: vector to return pointers to all leaf container
|
|
* in the tree.
|
|
* \param do_XOR_encoding_arg: select if binary tree structure should be generated
|
|
* based on current octree (false) of based on a XOR comparison between current and
|
|
* previous octree
|
|
* \param new_leafs_filter_arg: execute callback only for leaf nodes that did not
|
|
* exist in preceding buffer
|
|
**/
|
|
void
|
|
serializeTreeRecursive(
|
|
BranchNode* branch_arg,
|
|
OctreeKey& key_arg,
|
|
std::vector<char>* binary_tree_out_arg,
|
|
typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
|
|
bool do_XOR_encoding_arg = false,
|
|
bool new_leafs_filter_arg = false);
|
|
|
|
/** \brief Rebuild an octree based on binary XOR octree description and DataT objects
|
|
* for leaf node initialization.
|
|
* \param branch_arg: current branch node
|
|
* \param depth_mask_arg: depth mask used for octree key analysis and branch depth
|
|
* indicator
|
|
* \param key_arg: reference to an octree key
|
|
* \param binary_tree_in_it_arg iterator of binary input data
|
|
* \param binary_tree_in_it_end_arg
|
|
* \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers
|
|
* to be added to a leaf node
|
|
* \param leaf_container_vector_it_end_arg: iterator pointing to leaf container
|
|
* pointers pointing to last object in input container.
|
|
* \param branch_reset_arg: Reset pointer array of current branch
|
|
* \param do_XOR_decoding_arg: select if binary tree structure is based on current
|
|
* octree (false) of based on a XOR comparison between current and previous octree
|
|
**/
|
|
void
|
|
deserializeTreeRecursive(
|
|
BranchNode* branch_arg,
|
|
uindex_t depth_mask_arg,
|
|
OctreeKey& key_arg,
|
|
typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
|
|
typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
|
|
typename std::vector<LeafContainerT*>::const_iterator*
|
|
leaf_container_vector_it_arg,
|
|
typename std::vector<LeafContainerT*>::const_iterator*
|
|
leaf_container_vector_it_end_arg,
|
|
bool branch_reset_arg = false,
|
|
bool do_XOR_decoding_arg = false);
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Serialization callbacks
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/** \brief Callback executed for every leaf node data during serialization
|
|
**/
|
|
virtual void
|
|
serializeTreeCallback(LeafContainerT&, const OctreeKey&)
|
|
{}
|
|
|
|
/** \brief Callback executed for every leaf node data during deserialization
|
|
**/
|
|
virtual void
|
|
deserializeTreeCallback(LeafContainerT&, const OctreeKey&)
|
|
{}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Helpers
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/** \brief Recursively explore the octree and remove unused branch and leaf nodes
|
|
* \param branch_arg: current branch node
|
|
**/
|
|
void
|
|
treeCleanUpRecursive(BranchNode* branch_arg);
|
|
|
|
/** \brief Test if octree is able to dynamically change its depth. This is required
|
|
* for adaptive bounding box adjustment.
|
|
* \return "false" - not resizeable due to XOR serialization
|
|
**/
|
|
inline bool
|
|
octreeCanResize()
|
|
{
|
|
return (false);
|
|
}
|
|
|
|
/** \brief Prints binary representation of a byte - used for debugging
|
|
* \param data_arg - byte to be printed to stdout
|
|
**/
|
|
inline void
|
|
printBinary(char data_arg)
|
|
{
|
|
unsigned char mask = 1; // Bit mask
|
|
|
|
// Extract the bits
|
|
for (int i = 0; i < 8; i++) {
|
|
// Mask each bit in the byte and print it
|
|
std::cout << ((data_arg & (mask << i)) ? "1" : "0");
|
|
}
|
|
std::cout << std::endl;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// Globals
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/** \brief Amount of leaf nodes **/
|
|
std::size_t leaf_count_;
|
|
|
|
/** \brief Amount of branch nodes **/
|
|
std::size_t branch_count_;
|
|
|
|
/** \brief Pointer to root branch node of octree **/
|
|
BranchNode* root_node_;
|
|
|
|
/** \brief Depth mask based on octree depth **/
|
|
uindex_t depth_mask_;
|
|
|
|
/** \brief key range */
|
|
OctreeKey max_key_;
|
|
|
|
/** \brief Currently active octree buffer **/
|
|
unsigned char buffer_selector_;
|
|
|
|
/** \brief flags indicating if unused branches and leafs might exist in previous
|
|
* buffer **/
|
|
bool tree_dirty_flag_;
|
|
|
|
/** \brief Octree depth */
|
|
uindex_t octree_depth_;
|
|
|
|
/** \brief Enable dynamic_depth
|
|
* \note Note that this parameter is ignored in octree2buf! */
|
|
bool dynamic_depth_enabled_;
|
|
};
|
|
} // namespace octree
|
|
} // namespace pcl
|
|
|
|
#ifdef PCL_NO_PRECOMPILE
|
|
#include <pcl/octree/impl/octree2buf_base.hpp>
|
|
#endif
|