573 lines
19 KiB
C++
573 lines
19 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2011, 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$
|
|
*/
|
|
|
|
#ifndef PCL_OCTREE_BASE_HPP
|
|
#define PCL_OCTREE_BASE_HPP
|
|
|
|
#include <vector>
|
|
|
|
namespace pcl {
|
|
namespace octree {
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
OctreeBase<LeafContainerT, BranchContainerT>::OctreeBase()
|
|
: root_node_(new BranchNode())
|
|
{}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
OctreeBase<LeafContainerT, BranchContainerT>::~OctreeBase()
|
|
{
|
|
// deallocate tree structure
|
|
deleteTree();
|
|
delete (root_node_);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
|
|
uindex_t max_voxel_index_arg)
|
|
{
|
|
uindex_t tree_depth;
|
|
|
|
if (max_voxel_index_arg <= 0) {
|
|
PCL_ERROR("[pcl::octree::OctreeBase::setMaxVoxelIndex] Max voxel index (%lu) must "
|
|
"be > 0!\n",
|
|
max_voxel_index_arg);
|
|
return;
|
|
}
|
|
|
|
// tree depth == bitlength of maxVoxels
|
|
tree_depth =
|
|
std::min(static_cast<uindex_t>(OctreeKey::maxDepth),
|
|
static_cast<uindex_t>(std::ceil(std::log2(max_voxel_index_arg))));
|
|
|
|
setTreeDepth(tree_depth);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
|
|
{
|
|
if (depth_arg <= 0) {
|
|
PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
|
|
depth_arg);
|
|
return;
|
|
}
|
|
if (depth_arg > OctreeKey::maxDepth) {
|
|
PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be <= max "
|
|
"depth(%lu)!\n",
|
|
depth_arg,
|
|
OctreeKey::maxDepth);
|
|
return;
|
|
}
|
|
|
|
// set octree depth
|
|
octree_depth_ = depth_arg;
|
|
|
|
// define depth_mask_ by setting a single bit to 1 at bit position == tree depth
|
|
depth_mask_ = (1 << (depth_arg - 1));
|
|
|
|
// define max_key_
|
|
max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
LeafContainerT*
|
|
OctreeBase<LeafContainerT, BranchContainerT>::findLeaf(uindex_t idx_x_arg,
|
|
uindex_t idx_y_arg,
|
|
uindex_t idx_z_arg) const
|
|
{
|
|
// generate key
|
|
OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
|
|
|
|
// find the leaf node addressed by key
|
|
return (findLeaf(key));
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
LeafContainerT*
|
|
OctreeBase<LeafContainerT, BranchContainerT>::createLeaf(uindex_t idx_x_arg,
|
|
uindex_t idx_y_arg,
|
|
uindex_t idx_z_arg)
|
|
{
|
|
// generate key
|
|
OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
|
|
|
|
// create a leaf node addressed by key
|
|
return (createLeaf(key));
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
bool
|
|
OctreeBase<LeafContainerT, BranchContainerT>::existLeaf(uindex_t idx_x_arg,
|
|
uindex_t idx_y_arg,
|
|
uindex_t idx_z_arg) const
|
|
{
|
|
// generate key
|
|
OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
|
|
|
|
// check if key exist in octree
|
|
return (existLeaf(key));
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf(uindex_t idx_x_arg,
|
|
uindex_t idx_y_arg,
|
|
uindex_t idx_z_arg)
|
|
{
|
|
// generate key
|
|
OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
|
|
|
|
// delete the leaf node addressed by key
|
|
deleteLeafRecursive(key, depth_mask_, root_node_);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::deleteTree()
|
|
{
|
|
|
|
if (root_node_) {
|
|
// reset octree
|
|
deleteBranch(*root_node_);
|
|
leaf_count_ = 0;
|
|
branch_count_ = 1;
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::serializeTree(
|
|
std::vector<char>& binary_tree_out_arg) const
|
|
{
|
|
|
|
OctreeKey new_key;
|
|
|
|
// clear binary vector
|
|
binary_tree_out_arg.clear();
|
|
binary_tree_out_arg.reserve(this->branch_count_);
|
|
|
|
serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::serializeTree(
|
|
std::vector<char>& binary_tree_out_arg,
|
|
std::vector<LeafContainerT*>& leaf_container_vector_arg) const
|
|
{
|
|
|
|
OctreeKey new_key;
|
|
|
|
// clear output vectors
|
|
binary_tree_out_arg.clear();
|
|
leaf_container_vector_arg.clear();
|
|
|
|
binary_tree_out_arg.reserve(this->branch_count_);
|
|
leaf_container_vector_arg.reserve(this->leaf_count_);
|
|
|
|
serializeTreeRecursive(
|
|
root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs(
|
|
std::vector<LeafContainerT*>& leaf_container_vector_arg)
|
|
{
|
|
OctreeKey new_key;
|
|
|
|
// clear output vector
|
|
leaf_container_vector_arg.clear();
|
|
|
|
leaf_container_vector_arg.reserve(this->leaf_count_);
|
|
|
|
serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
|
|
std::vector<char>& binary_tree_out_arg)
|
|
{
|
|
OctreeKey new_key;
|
|
|
|
// free existing tree before tree rebuild
|
|
deleteTree();
|
|
|
|
// iterator for binary tree structure vector
|
|
std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
|
|
std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
|
|
|
|
deserializeTreeRecursive(root_node_,
|
|
depth_mask_,
|
|
new_key,
|
|
binary_tree_out_it,
|
|
binary_tree_out_it_end,
|
|
nullptr,
|
|
nullptr);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
|
|
std::vector<char>& binary_tree_in_arg,
|
|
std::vector<LeafContainerT*>& leaf_container_vector_arg)
|
|
{
|
|
OctreeKey new_key;
|
|
|
|
// set data iterator to first element
|
|
typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
|
|
leaf_container_vector_arg.begin();
|
|
|
|
// set data iterator to last element
|
|
typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
|
|
leaf_container_vector_arg.end();
|
|
|
|
// free existing tree before tree rebuild
|
|
deleteTree();
|
|
|
|
// iterator for binary tree structure vector
|
|
std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
|
|
std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
|
|
|
|
deserializeTreeRecursive(root_node_,
|
|
depth_mask_,
|
|
new_key,
|
|
binary_tree_input_it,
|
|
binary_tree_input_it_end,
|
|
&leaf_vector_it,
|
|
&leaf_vector_it_end);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
uindex_t
|
|
OctreeBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
|
|
const OctreeKey& key_arg,
|
|
uindex_t depth_mask_arg,
|
|
BranchNode* branch_arg,
|
|
LeafNode*& return_leaf_arg,
|
|
BranchNode*& parent_of_leaf_arg)
|
|
{
|
|
// index to branch child
|
|
unsigned char child_idx;
|
|
|
|
// find branch child from key
|
|
child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
|
|
|
|
OctreeNode* child_node = (*branch_arg)[child_idx];
|
|
|
|
if (!child_node) {
|
|
if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
|
|
// if required branch does not exist -> create it
|
|
BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
|
|
|
|
branch_count_++;
|
|
|
|
// recursively proceed with indexed child branch
|
|
return createLeafRecursive(key_arg,
|
|
depth_mask_arg >> 1,
|
|
childBranch,
|
|
return_leaf_arg,
|
|
parent_of_leaf_arg);
|
|
}
|
|
// if leaf node at child_idx does not exist
|
|
LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
|
|
return_leaf_arg = leaf_node;
|
|
parent_of_leaf_arg = branch_arg;
|
|
this->leaf_count_++;
|
|
}
|
|
else {
|
|
// Node exists already
|
|
switch (child_node->getNodeType()) {
|
|
case BRANCH_NODE:
|
|
// recursively proceed with indexed child branch
|
|
return createLeafRecursive(key_arg,
|
|
depth_mask_arg >> 1,
|
|
static_cast<BranchNode*>(child_node),
|
|
return_leaf_arg,
|
|
parent_of_leaf_arg);
|
|
break;
|
|
|
|
case LEAF_NODE:
|
|
return_leaf_arg = static_cast<LeafNode*>(child_node);
|
|
parent_of_leaf_arg = branch_arg;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return (depth_mask_arg >> 1);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
|
|
const OctreeKey& key_arg,
|
|
uindex_t depth_mask_arg,
|
|
BranchNode* branch_arg,
|
|
LeafContainerT*& result_arg) const
|
|
{
|
|
// index to branch child
|
|
unsigned char child_idx;
|
|
|
|
// find branch child from key
|
|
child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
|
|
|
|
OctreeNode* child_node = (*branch_arg)[child_idx];
|
|
|
|
if (child_node) {
|
|
switch (child_node->getNodeType()) {
|
|
case BRANCH_NODE:
|
|
// we have not reached maximum tree depth
|
|
BranchNode* child_branch;
|
|
child_branch = static_cast<BranchNode*>(child_node);
|
|
|
|
findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
|
|
break;
|
|
|
|
case LEAF_NODE:
|
|
// return existing leaf node
|
|
LeafNode* child_leaf;
|
|
child_leaf = static_cast<LeafNode*>(child_node);
|
|
|
|
result_arg = child_leaf->getContainerPtr();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
bool
|
|
OctreeBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
|
|
const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
|
|
{
|
|
// index to branch child
|
|
unsigned char child_idx;
|
|
// indicates if branch has children, if so, it can't be removed
|
|
bool b_has_children;
|
|
|
|
// find branch child from key
|
|
child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
|
|
|
|
OctreeNode* child_node = (*branch_arg)[child_idx];
|
|
|
|
if (child_node) {
|
|
switch (child_node->getNodeType()) {
|
|
|
|
case BRANCH_NODE:
|
|
BranchNode* child_branch;
|
|
child_branch = static_cast<BranchNode*>(child_node);
|
|
|
|
// recursively explore the indexed child branch
|
|
b_has_children = deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
|
|
|
|
if (!b_has_children) {
|
|
// child branch does not own any sub-child nodes anymore -> delete child branch
|
|
deleteBranchChild(*branch_arg, child_idx);
|
|
branch_count_--;
|
|
}
|
|
break;
|
|
|
|
case LEAF_NODE:
|
|
// return existing leaf node
|
|
|
|
// our child is a leaf node -> delete it
|
|
deleteBranchChild(*branch_arg, child_idx);
|
|
this->leaf_count_--;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// check if current branch still owns children
|
|
b_has_children = false;
|
|
for (child_idx = 0; (!b_has_children) && (child_idx < 8); child_idx++) {
|
|
b_has_children = branch_arg->hasChild(child_idx);
|
|
}
|
|
// return false if current branch can be deleted
|
|
return (b_has_children);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive(
|
|
const BranchNode* branch_arg,
|
|
OctreeKey& key_arg,
|
|
std::vector<char>* binary_tree_out_arg,
|
|
typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
|
|
{
|
|
char node_bit_pattern;
|
|
|
|
// branch occupancy bit pattern
|
|
node_bit_pattern = getBranchBitPattern(*branch_arg);
|
|
|
|
// write bit pattern to output vector
|
|
if (binary_tree_out_arg)
|
|
binary_tree_out_arg->push_back(node_bit_pattern);
|
|
|
|
// iterate over all children
|
|
for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
|
|
|
|
// if child exist
|
|
if (branch_arg->hasChild(child_idx)) {
|
|
// add current branch voxel to key
|
|
key_arg.pushBranch(child_idx);
|
|
|
|
OctreeNode* childNode = branch_arg->getChildPtr(child_idx);
|
|
|
|
switch (childNode->getNodeType()) {
|
|
case BRANCH_NODE: {
|
|
// recursively proceed with indexed child branch
|
|
serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
|
|
key_arg,
|
|
binary_tree_out_arg,
|
|
leaf_container_vector_arg);
|
|
break;
|
|
}
|
|
case LEAF_NODE: {
|
|
auto* child_leaf = static_cast<LeafNode*>(childNode);
|
|
|
|
if (leaf_container_vector_arg)
|
|
leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
|
|
|
|
// we reached a leaf node -> execute serialization callback
|
|
serializeTreeCallback(**child_leaf, key_arg);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// pop current branch voxel from key
|
|
key_arg.popBranch();
|
|
}
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename LeafContainerT, typename BranchContainerT>
|
|
void
|
|
OctreeBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
|
|
BranchNode* branch_arg,
|
|
uindex_t depth_mask_arg,
|
|
OctreeKey& key_arg,
|
|
typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
|
|
typename std::vector<char>::const_iterator& binary_tree_input_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)
|
|
{
|
|
|
|
if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
|
|
// read branch occupancy bit pattern from input vector
|
|
char node_bits = (*binary_tree_input_it_arg);
|
|
binary_tree_input_it_arg++;
|
|
|
|
// iterate over all children
|
|
for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
|
|
// if occupancy bit for child_idx is set..
|
|
if (node_bits & (1 << child_idx)) {
|
|
// add current branch voxel to key
|
|
key_arg.pushBranch(child_idx);
|
|
|
|
if (depth_mask_arg > 1) {
|
|
// we have not reached maximum tree depth
|
|
BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
|
|
|
|
branch_count_++;
|
|
|
|
// recursively proceed with new child branch
|
|
deserializeTreeRecursive(newBranch,
|
|
depth_mask_arg >> 1,
|
|
key_arg,
|
|
binary_tree_input_it_arg,
|
|
binary_tree_input_it_end_arg,
|
|
leaf_container_vector_it_arg,
|
|
leaf_container_vector_it_end_arg);
|
|
}
|
|
else {
|
|
// we reached leaf node level
|
|
|
|
LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
|
|
|
|
if (leaf_container_vector_it_arg &&
|
|
(*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) {
|
|
LeafContainerT& container = **child_leaf;
|
|
LeafContainerT* src_container_ptr = **leaf_container_vector_it_arg;
|
|
container = *src_container_ptr;
|
|
++*leaf_container_vector_it_arg;
|
|
}
|
|
|
|
leaf_count_++;
|
|
|
|
// execute deserialization callback
|
|
deserializeTreeCallback(**child_leaf, key_arg);
|
|
}
|
|
|
|
// pop current branch voxel from key
|
|
key_arg.popBranch();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace octree
|
|
} // namespace pcl
|
|
|
|
#define PCL_INSTANTIATE_OctreeBase(T) \
|
|
template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
|
|
|
|
#endif
|