Point Cloud Library (PCL)  1.11.1
approximate_voxel_grid.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: voxel_grid.hpp 1600 2011-07-07 16:55:51Z shapovalov $
35  *
36  */
37 
38 #ifndef PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_
39 #define PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_
40 
41 #include <pcl/common/io.h>
42 #include <pcl/filters/approximate_voxel_grid.h>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointT> void
46 pcl::ApproximateVoxelGrid<PointT>::flush (PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size)
47 {
48  hhe->centroid /= static_cast<float> (hhe->count);
49  pcl::for_each_type <FieldList> (pcl::xNdCopyEigenPointFunctor <PointT> (hhe->centroid, output[op]));
50  // ---[ RGB special case
51  if (rgba_index >= 0)
52  {
53  // pack r/g/b into rgb
54  float r = hhe->centroid[centroid_size-3],
55  g = hhe->centroid[centroid_size-2],
56  b = hhe->centroid[centroid_size-1];
57  int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
58  memcpy (reinterpret_cast<char*> (&output[op]) + rgba_index, &rgb, sizeof (float));
59  }
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT> void
65 {
66  int centroid_size = 4;
67  if (downsample_all_data_)
68  centroid_size = boost::mpl::size<FieldList>::value;
69 
70  // ---[ RGB special case
71  std::vector<pcl::PCLPointField> fields;
72  int rgba_index = -1;
73  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
74  if (rgba_index == -1)
75  rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
76  if (rgba_index >= 0)
77  {
78  rgba_index = fields[rgba_index].offset;
79  centroid_size += 3;
80  }
81 
82  for (std::size_t i = 0; i < histsize_; i++)
83  {
84  history_[i].count = 0;
85  history_[i].centroid = Eigen::VectorXf::Zero (centroid_size);
86  }
87  Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size);
88 
89  output.points.resize (input_->size ()); // size output for worst case
90  std::size_t op = 0; // output pointer
91  for (const auto& point: *input_)
92  {
93  int ix = static_cast<int> (std::floor (point.x * inverse_leaf_size_[0]));
94  int iy = static_cast<int> (std::floor (point.y * inverse_leaf_size_[1]));
95  int iz = static_cast<int> (std::floor (point.z * inverse_leaf_size_[2]));
96  unsigned int hash = static_cast<unsigned int> ((ix * 7171 + iy * 3079 + iz * 4231) & (histsize_ - 1));
97  he *hhe = &history_[hash];
98  if (hhe->count && ((ix != hhe->ix) || (iy != hhe->iy) || (iz != hhe->iz)))
99  {
100  flush (output, op++, hhe, rgba_index, centroid_size);
101  hhe->count = 0;
102  hhe->centroid.setZero ();// = Eigen::VectorXf::Zero (centroid_size);
103  }
104  hhe->ix = ix;
105  hhe->iy = iy;
106  hhe->iz = iz;
107  hhe->count++;
108 
109  // Unpack the point into scratch, then accumulate
110  // ---[ RGB special case
111  if (rgba_index >= 0)
112  {
113  // fill r/g/b data
114  pcl::RGB rgb;
115  memcpy (&rgb, (reinterpret_cast<const char *> (&point)) + rgba_index, sizeof (RGB));
116  scratch[centroid_size-3] = rgb.r;
117  scratch[centroid_size-2] = rgb.g;
118  scratch[centroid_size-1] = rgb.b;
119  }
120  pcl::for_each_type <FieldList> (xNdCopyPointEigenFunctor <PointT> (point, scratch));
121  hhe->centroid += scratch;
122  }
123  for (std::size_t i = 0; i < histsize_; i++)
124  {
125  he *hhe = &history_[i];
126  if (hhe->count)
127  flush (output, op++, hhe, rgba_index, centroid_size);
128  }
129  output.points.resize (op);
130  output.width = output.size ();
131  output.height = 1; // downsampling breaks the organized structure
132  output.is_dense = false; // we filter out invalid points
133 }
134 
135 #define PCL_INSTANTIATE_ApproximateVoxelGrid(T) template class PCL_EXPORTS pcl::ApproximateVoxelGrid<T>;
136 
137 #endif // PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
void flush(PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size)
Write a single point from the hash to the output cloud.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:419
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
std::size_t size() const
Definition: point_cloud.h:459
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
A structure representing RGB color information.
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.