Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
octree_pointcloud_voxelcentroid.h
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$
38 */
39
40#pragma once
41
42#include <pcl/octree/octree_pointcloud.h>
43
44namespace pcl {
45namespace octree {
46/** \brief @b Octree pointcloud voxel centroid leaf node class
47 * \note This class implements a leaf node that calculates the mean centroid of all
48 * points added this octree container.
49 * \author Julius Kammerl (julius@kammerl.de)
50 */
51template <typename PointT>
53public:
54 /** \brief Class initialization. */
56
57 /** \brief Empty class deconstructor. */
59
60 /** \brief deep copy function */
62 deepCopy() const
63 {
65 }
66
67 /** \brief Equal comparison operator - set to false
68 */
69 // param[in] OctreePointCloudVoxelCentroidContainer to compare with
70 bool
71 operator==(const OctreeContainerBase&) const override
72 {
73 return (false);
74 }
75
76 /** \brief Add new point to voxel.
77 * \param[in] new_point the new point to add
78 */
79 void
80 addPoint(const PointT& new_point)
81 {
82 using namespace pcl::common;
83
84 ++point_counter_;
85
86 point_sum_ += new_point;
87 }
88
89 /** \brief Calculate centroid of voxel.
90 * \param[out] centroid_arg the resultant centroid of the voxel
91 */
92 void
93 getCentroid(PointT& centroid_arg) const
94 {
95 using namespace pcl::common;
96
97 if (point_counter_) {
98 centroid_arg = point_sum_;
99 centroid_arg /= static_cast<float>(point_counter_);
100 }
101 else {
102 centroid_arg *= 0.0f;
103 }
104 }
105
106 /** \brief Reset leaf container. */
107 void
108 reset() override
109 {
110 using namespace pcl::common;
111
112 point_counter_ = 0;
113 point_sum_ *= 0.0f;
114 }
115
116private:
117 uindex_t point_counter_;
118 PointT point_sum_;
119};
120
121/** \brief @b Octree pointcloud voxel centroid class
122 * \note This class generate an octrees from a point cloud (zero-copy). It provides a
123 * vector of centroids for all occupied voxels.
124 * \note The octree pointcloud is initialized with its voxel resolution. Its bounding
125 * box is automatically adjusted or can be predefined.
126 * \tparam PointT type of point used in pointcloud
127 * \ingroup octree
128 * \author Julius Kammerl (julius@kammerl.de)
129 */
130template <typename PointT,
131 typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT>,
132 typename BranchContainerT = OctreeContainerEmpty>
134: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
135public:
136 using Ptr = shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT>>;
137 using ConstPtr =
138 shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT>>;
139
141 using LeafNode = typename OctreeT::LeafNode;
143
144 /** \brief OctreePointCloudVoxelCentroids class constructor.
145 * \param[in] resolution_arg octree resolution at lowest octree level
146 */
147 OctreePointCloudVoxelCentroid(const double resolution_arg)
148 : OctreePointCloud<PointT, LeafContainerT, BranchContainerT>(resolution_arg)
149 {}
150
151 /** \brief Empty class deconstructor. */
152
154
155 /** \brief Add DataT object to leaf node at octree key.
156 * \param pointIdx_arg
157 */
158 void
159 addPointIdx(const uindex_t pointIdx_arg) override
160 {
161 OctreeKey key;
162
163 assert(pointIdx_arg < this->input_->size());
164
165 const PointT& point = (*this->input_)[pointIdx_arg];
166
167 // make sure bounding box is big enough
168 this->adoptBoundingBoxToPoint(point);
169
170 // generate key
171 this->genOctreeKeyforPoint(point, key);
172
173 // add point to octree at key
174 LeafContainerT* container = this->createLeaf(key);
175 container->addPoint(point);
176 }
177
178 /** \brief Get centroid for a single voxel addressed by a PointT point.
179 * \param[in] point_arg point addressing a voxel in octree
180 * \param[out] voxel_centroid_arg centroid is written to this PointT reference
181 * \return "true" if voxel is found; "false" otherwise
182 */
183 bool
184 getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const;
185
186 /** \brief Get centroid for a single voxel addressed by a PointT point from input
187 * cloud.
188 * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree
189 * \param[out] voxel_centroid_arg centroid is written to this PointT reference
190 * \return "true" if voxel is found; "false" otherwise
191 */
192 inline bool
193 getVoxelCentroidAtPoint(const index_t& point_idx_arg,
194 PointT& voxel_centroid_arg) const
195 {
196 // get centroid at point
197 return (this->getVoxelCentroidAtPoint((*this->input_)[point_idx_arg],
198 voxel_centroid_arg));
199 }
200
201 /** \brief Get PointT vector of centroids for all occupied voxels.
202 * \param[out] voxel_centroid_list_arg results are written to this vector of PointT
203 * elements
204 * \return number of occupied voxels
205 */
209 AlignedPointTVector& voxel_centroid_list_arg) const;
210
211 /** \brief Recursively explore the octree and output a PointT vector of centroids for
212 * all occupied voxels.
213 * \param[in] branch_arg: current branch node
214 * \param[in] key_arg: current key
215 * \param[out] voxel_centroid_list_arg results are written to this vector of PointT
216 * elements
217 */
218 void
220 const BranchNode* branch_arg,
221 OctreeKey& key_arg,
223 AlignedPointTVector& voxel_centroid_list_arg) const;
224};
225} // namespace octree
226} // namespace pcl
227
228// Note: Don't precompile this octree type to speed up compilation. It's probably rarely
229// used.
230#include <pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp>
LeafContainerT * createLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg)
Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
Octree container class that can serve as a base to construct own leaf node container classes.
Octree key class
Definition octree_key.h:52
Octree pointcloud class
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
PointCloudConstPtr input_
Pointer to input point cloud dataset.
typename OctreeT::LeafNode LeafNode
typename OctreeT::BranchNode BranchNode
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
Octree pointcloud voxel centroid leaf node class
virtual OctreePointCloudVoxelCentroidContainer * deepCopy() const
deep copy function
void addPoint(const PointT &new_point)
Add new point to voxel.
void getCentroid(PointT &centroid_arg) const
Calculate centroid of voxel.
bool operator==(const OctreeContainerBase &) const override
Equal comparison operator - set to false.
bool getVoxelCentroidAtPoint(const PointT &point_arg, PointT &voxel_centroid_arg) const
Get centroid for a single voxel addressed by a PointT point.
OctreePointCloudVoxelCentroid(const double resolution_arg)
OctreePointCloudVoxelCentroids class constructor.
shared_ptr< const OctreePointCloudVoxelCentroid< PointT, LeafContainerT > > ConstPtr
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.
void addPointIdx(const uindex_t pointIdx_arg) override
Add DataT object to leaf node at octree key.
bool getVoxelCentroidAtPoint(const index_t &point_idx_arg, PointT &voxel_centroid_arg) const
Get centroid for a single voxel addressed by a PointT point from input cloud.
shared_ptr< OctreePointCloudVoxelCentroid< PointT, LeafContainerT > > Ptr
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
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.