Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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#include <boost/mpl/size.hpp> // for size
44
45//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46template <typename PointT> void
47pcl::ApproximateVoxelGrid<PointT>::flush (PointCloud &output, std::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[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[op]) + rgba_index, &rgb, sizeof (float));
60 }
61}
62
63//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64template <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<PointT> ("rgb", fields);
75 if (rgba_index == -1)
76 rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
77 if (rgba_index >= 0)
78 {
79 rgba_index = fields[rgba_index].offset;
80 centroid_size += 3;
81 }
82
83 for (std::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.resize (input_->size ()); // size output for worst case
91 std::size_t op = 0; // output pointer
92 for (const auto& point: *input_)
93 {
94 int ix = static_cast<int> (std::floor (point.x * inverse_leaf_size_[0]));
95 int iy = static_cast<int> (std::floor (point.y * inverse_leaf_size_[1]));
96 int iz = static_cast<int> (std::floor (point.z * inverse_leaf_size_[2]));
97 auto 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 *> (&point)) + 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> (point, scratch));
122 hhe->centroid += scratch;
123 }
124 for (std::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.resize (op);
131 output.width = output.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_
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.
A structure representing RGB color information.