octree_pointcloud_voxelcentroid.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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: octree_pointcloud_voxelcentroid.hpp 6459 2012-07-18 07:50:37Z dpb $
*/
#ifndef PCL_OCTREE_VOXELCENTROID_HPP
#define PCL_OCTREE_VOXELCENTROID_HPP
/*
* OctreePointCloudVoxelcontroid is not precompiled, since it's used in other
* parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT
* used, octree_pointcloud_voxelcentroid.h includes this file but octree_pointcloud.h
* would not include the implementation because it's precompiled. So we need to
* include it here "manually".
*/
#include <pcl/octree/impl/octree_pointcloud.hpp>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
bool
pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const
{
OctreeKey key;
LeafContainerT* leaf = NULL;
// generate key
genOctreeKeyforPoint(point_arg, key);
leaf = this->findLeaf(key);
if (leaf)
leaf->getCentroid(voxel_centroid_arg);
return (leaf != NULL);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
pcl::uindex_t
pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
getVoxelCentroids(
typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
AlignedPointTVector& voxel_centroid_list_arg) const
{
OctreeKey new_key;
// reset output vector
voxel_centroid_list_arg.clear();
voxel_centroid_list_arg.reserve(this->leaf_count_);
getVoxelCentroidsRecursive(this->root_node_, new_key, voxel_centroid_list_arg);
// return size of centroid vector
return (voxel_centroid_list_arg.size());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::
getVoxelCentroidsRecursive(
const BranchNode* branch_arg,
OctreeKey& key_arg,
typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::
AlignedPointTVector& voxel_centroid_list_arg) const
{
// 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* child_node = branch_arg->getChildPtr(child_idx);
switch (child_node->getNodeType()) {
case BRANCH_NODE: {
// recursively proceed with indexed child branch
getVoxelCentroidsRecursive(static_cast<const BranchNode*>(child_node),
key_arg,
voxel_centroid_list_arg);
break;
}
case LEAF_NODE: {
PointT new_centroid;
LeafNode* container = static_cast<LeafNode*>(child_node);
container->getContainer().getCentroid(new_centroid);
voxel_centroid_list_arg.push_back(new_centroid);
break;
}
default:
break;
}
// pop current branch voxel from key
key_arg.popBranch();
}
}
}
#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) \
template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
#endif
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:179
pcl::octree::OctreeBranchNode::hasChild
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
Definition: octree_nodes.h:258
pcl::octree::OctreeBranchNode::getChildPtr
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
Definition: octree_nodes.h:237
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:52
pcl::octree::OctreeKey::popBranch
void popBranch()
pop child node from octree key
Definition: octree_key.h:120
pcl::octree::OctreeKey::pushBranch
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:110
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:80
pcl::octree::OctreeLeafNode::getContainer
const ContainerT & getContainer() const
Get const reference to container.
Definition: octree_nodes.h:139
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:58
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:72
pcl::octree::OctreePointCloudVoxelCentroid::getVoxelCentroidAtPoint
bool getVoxelCentroidAtPoint(const PointT &point_arg, PointT &voxel_centroid_arg) const
Get centroid for a single voxel addressed by a PointT point.
Definition: octree_pointcloud_voxelcentroid.hpp:56
pcl::octree::OctreePointCloudVoxelCentroid::getVoxelCentroidsRecursive
void getVoxelCentroidsRecursive(const BranchNode *branch_arg, OctreeKey &key_arg, typename OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::AlignedPointTVector &voxel_centroid_list_arg) const
Recursively explore the octree and output a PointT vector of centroids for all occupied voxels.
Definition: octree_pointcloud_voxelcentroid.hpp:95
pcl::octree::OctreePointCloudVoxelCentroid::getVoxelCentroids
uindex_t getVoxelCentroids(typename OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::AlignedPointTVector &voxel_centroid_list_arg) const
Get PointT vector of centroids for all occupied voxels.
Definition: octree_pointcloud_voxelcentroid.hpp:75
pcl::octree::BRANCH_NODE
@ BRANCH_NODE
Definition: octree_nodes.h:51
pcl::octree::LEAF_NODE
@ LEAF_NODE
Definition: octree_nodes.h:51
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:675