Point Cloud Library (PCL)  1.9.1-dev
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/common.h>
42 #include <pcl/common/io.h>
43 #include <pcl/filters/approximate_voxel_grid.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointT> void
47 pcl::ApproximateVoxelGrid<PointT>::flush (PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size)
48 {
49  hhe->centroid /= static_cast<float> (hhe->count);
50  pcl::for_each_type <FieldList> (pcl::xNdCopyEigenPointFunctor <PointT> (hhe->centroid, output.points[op]));
51  // ---[ RGB special case
52  if (rgba_index >= 0)
53  {
54  // pack r/g/b into rgb
55  float r = hhe->centroid[centroid_size-3],
56  g = hhe->centroid[centroid_size-2],
57  b = hhe->centroid[centroid_size-1];
58  int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
59  memcpy (reinterpret_cast<char*> (&output.points[op]) + rgba_index, &rgb, sizeof (float));
60  }
61 }
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointT> void
66 {
67  int centroid_size = 4;
68  if (downsample_all_data_)
69  centroid_size = boost::mpl::size<FieldList>::value;
70 
71  // ---[ RGB special case
72  std::vector<pcl::PCLPointField> fields;
73  int rgba_index = -1;
74  rgba_index = pcl::getFieldIndex (*input_, "rgb", fields);
75  if (rgba_index == -1)
76  rgba_index = pcl::getFieldIndex (*input_, "rgba", fields);
77  if (rgba_index >= 0)
78  {
79  rgba_index = fields[rgba_index].offset;
80  centroid_size += 3;
81  }
82 
83  for (size_t i = 0; i < histsize_; i++)
84  {
85  history_[i].count = 0;
86  history_[i].centroid = Eigen::VectorXf::Zero (centroid_size);
87  }
88  Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size);
89 
90  output.points.resize (input_->points.size ()); // size output for worst case
91  size_t op = 0; // output pointer
92  for (size_t cp = 0; cp < input_->points.size (); ++cp)
93  {
94  int ix = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]));
95  int iy = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]));
96  int iz = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]));
97  unsigned int hash = static_cast<unsigned int> ((ix * 7171 + iy * 3079 + iz * 4231) & (histsize_ - 1));
98  he *hhe = &history_[hash];
99  if (hhe->count && ((ix != hhe->ix) || (iy != hhe->iy) || (iz != hhe->iz)))
100  {
101  flush (output, op++, hhe, rgba_index, centroid_size);
102  hhe->count = 0;
103  hhe->centroid.setZero ();// = Eigen::VectorXf::Zero (centroid_size);
104  }
105  hhe->ix = ix;
106  hhe->iy = iy;
107  hhe->iz = iz;
108  hhe->count++;
109 
110  // Unpack the point into scratch, then accumulate
111  // ---[ RGB special case
112  if (rgba_index >= 0)
113  {
114  // fill r/g/b data
115  pcl::RGB rgb;
116  memcpy (&rgb, (reinterpret_cast<const char *> (&input_->points[cp])) + rgba_index, sizeof (RGB));
117  scratch[centroid_size-3] = rgb.r;
118  scratch[centroid_size-2] = rgb.g;
119  scratch[centroid_size-1] = rgb.b;
120  }
121  pcl::for_each_type <FieldList> (xNdCopyPointEigenFunctor <PointT> (input_->points[cp], scratch));
122  hhe->centroid += scratch;
123  }
124  for (size_t i = 0; i < histsize_; i++)
125  {
126  he *hhe = &history_[i];
127  if (hhe->count)
128  flush (output, op++, hhe, rgba_index, centroid_size);
129  }
130  output.points.resize (op);
131  output.width = static_cast<uint32_t> (output.points.size ());
132  output.height = 1; // downsampling breaks the organized structure
133  output.is_dense = false; // we filter out invalid points
134 }
135 
136 #define PCL_INSTANTIATE_ApproximateVoxelGrid(T) template class PCL_EXPORTS pcl::ApproximateVoxelGrid<T>;
137 
138 #endif // PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:58
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
void flush(PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size)
Write a single point from the hash to the output cloud.
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
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
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.