Point Cloud Library (PCL)  1.14.1-dev
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 
44 namespace pcl {
45 namespace 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  */
51 template <typename PointT>
53 public:
54  /** \brief Class initialization. */
56 
57  /** \brief Empty class deconstructor. */
59 
60  /** \brief deep copy function */
62  deepCopy() const
63  {
64  return (new OctreePointCloudVoxelCentroidContainer(*this));
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 
116 private:
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  */
130 template <typename PointT,
131  typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT>,
132  typename BranchContainerT = OctreeContainerEmpty>
134 : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
135 public:
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;
142  using BranchNode = typename OctreeT::BranchNode;
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 
153  ~OctreePointCloudVoxelCentroid() override = default;
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  */
206  uindex_t
207  getVoxelCentroids(
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
219  getVoxelCentroidsRecursive(
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>
Abstract octree branch class
Definition: octree_nodes.h:179
Octree container class that can serve as a base to construct own leaf node container classes.
Octree key class
Definition: octree_key.h:54
Abstract octree leaf class
Definition: octree_nodes.h:81
Octree pointcloud class
Octree pointcloud voxel centroid leaf node class
void addPoint(const PointT &new_point)
Add new point to voxel.
void getCentroid(PointT &centroid_arg) const
Calculate centroid of voxel.
~OctreePointCloudVoxelCentroidContainer() override=default
Empty class deconstructor.
virtual OctreePointCloudVoxelCentroidContainer * deepCopy() const
deep copy function
bool operator==(const OctreeContainerBase &) const override
Equal comparison operator - set to false.
OctreePointCloudVoxelCentroid(const double resolution_arg)
OctreePointCloudVoxelCentroids class constructor.
shared_ptr< const OctreePointCloudVoxelCentroid< PointT, LeafContainerT > > ConstPtr
void addPointIdx(const uindex_t pointIdx_arg) override
Add DataT object to leaf node at octree key.
shared_ptr< OctreePointCloudVoxelCentroid< PointT, LeafContainerT > > Ptr
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.
~OctreePointCloudVoxelCentroid() override=default
Empty class deconstructor.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
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
A point structure representing Euclidean xyz coordinates, and the RGB color.