Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
octree_pointcloud_voxelcentroid.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: octree_pointcloud_voxelcentroid.hpp 6459 2012-07-18 07:50:37Z dpb $
38 */
39
40#ifndef PCL_OCTREE_VOXELCENTROID_HPP
41#define PCL_OCTREE_VOXELCENTROID_HPP
42
43/*
44 * OctreePointCloudVoxelcontroid is not precompiled, since it's used in other
45 * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT
46 * used, octree_pointcloud_voxelcentroid.h includes this file but octree_pointcloud.h
47 * would not include the implementation because it's precompiled. So we need to
48 * include it here "manually".
49 */
50#include <pcl/octree/impl/octree_pointcloud.hpp>
51
52//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53template <typename PointT, typename LeafContainerT, typename BranchContainerT>
54bool
56 getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const
57{
58 OctreeKey key;
59 LeafContainerT* leaf = NULL;
60
61 // generate key
62 genOctreeKeyforPoint(point_arg, key);
63
64 leaf = this->findLeaf(key);
65 if (leaf)
66 leaf->getCentroid(voxel_centroid_arg);
67
68 return (leaf != NULL);
69}
70
71//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
72template <typename PointT, typename LeafContainerT, typename BranchContainerT>
77 AlignedPointTVector& voxel_centroid_list_arg) const
78{
79 OctreeKey new_key;
80
81 // reset output vector
82 voxel_centroid_list_arg.clear();
83 voxel_centroid_list_arg.reserve(this->leaf_count_);
84
85 getVoxelCentroidsRecursive(this->root_node_, new_key, voxel_centroid_list_arg);
86
87 // return size of centroid vector
88 return (voxel_centroid_list_arg.size());
89}
90
91//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
92template <typename PointT, typename LeafContainerT, typename BranchContainerT>
93void
96 const BranchNode* branch_arg,
97 OctreeKey& key_arg,
99 AlignedPointTVector& voxel_centroid_list_arg) const
100{
101 // iterate over all children
102 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
103 // if child exist
104 if (branch_arg->hasChild(child_idx)) {
105 // add current branch voxel to key
106 key_arg.pushBranch(child_idx);
107
108 OctreeNode* child_node = branch_arg->getChildPtr(child_idx);
109
110 switch (child_node->getNodeType()) {
111 case BRANCH_NODE: {
112 // recursively proceed with indexed child branch
113 getVoxelCentroidsRecursive(static_cast<const BranchNode*>(child_node),
114 key_arg,
115 voxel_centroid_list_arg);
116 break;
117 }
118 case LEAF_NODE: {
119 PointT new_centroid;
120
121 LeafNode* container = static_cast<LeafNode*>(child_node);
122
123 container->getContainer().getCentroid(new_centroid);
124
125 voxel_centroid_list_arg.push_back(new_centroid);
126 break;
127 }
128 default:
129 break;
130 }
131
132 // pop current branch voxel from key
133 key_arg.popBranch();
134 }
135 }
136}
137
138#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) \
139 template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid<T>;
140
141#endif
std::size_t leaf_count_
Amount of leaf nodes
Definition octree_base.h:78
BranchNode * root_node_
Pointer to root branch node of octree
Definition octree_base.h:84
Octree key class
Definition octree_key.h:52
void popBranch()
pop child node from octree key
Definition octree_key.h:120
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition octree_key.h:110
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
Octree pointcloud class
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
bool getVoxelCentroidAtPoint(const PointT &point_arg, PointT &voxel_centroid_arg) const
Get centroid for a single voxel addressed by a PointT point.
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.
uindex_t getVoxelCentroids(typename OctreePointCloud< PointT, LeafContainerT, BranchContainerT >::AlignedPointTVector &voxel_centroid_list_arg) const
Get PointT vector of centroids for all occupied voxels.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
A point structure representing Euclidean xyz coordinates, and the RGB color.