From 3a9ef064fe058b512d8c0eaac1ea323419490e89 Mon Sep 17 00:00:00 2001 From: Bradley J Chambers Date: Tue, 18 Feb 2014 18:54:38 +0000 Subject: [PATCH 1/2] Add: create local 2D grid over input point cloud and downsample to minimum z values per grid cell --- filters/CMakeLists.txt | 3 + filters/include/pcl/filters/grid_minimum.h | 208 ++++++++++++++++++ .../include/pcl/filters/impl/grid_minimum.hpp | 200 +++++++++++++++++ filters/src/grid_minimum.cpp | 52 +++++ test/filters/CMakeLists.txt | 3 + test/filters/test_grid_minimum.cpp | 80 +++++++ 6 files changed, 546 insertions(+) create mode 100644 filters/include/pcl/filters/grid_minimum.h create mode 100644 filters/include/pcl/filters/impl/grid_minimum.hpp create mode 100644 filters/src/grid_minimum.cpp create mode 100644 test/filters/test_grid_minimum.cpp diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt index 8dc7f0e58a1..e35e91a73f9 100644 --- a/filters/CMakeLists.txt +++ b/filters/CMakeLists.txt @@ -36,6 +36,7 @@ if(build) src/median_filter.cpp src/voxel_grid_occlusion_estimation.cpp src/normal_refinement.cpp + src/grid_minimum.cpp ) set(incs @@ -71,6 +72,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/covariance_sampling.h" "include/pcl/${SUBSYS_NAME}/median_filter.h" "include/pcl/${SUBSYS_NAME}/normal_refinement.h" + "include/pcl/${SUBSYS_NAME}/grid_minimum.h" ) set(impl_incs @@ -103,6 +105,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/covariance_sampling.hpp" "include/pcl/${SUBSYS_NAME}/impl/median_filter.hpp" "include/pcl/${SUBSYS_NAME}/impl/normal_refinement.hpp" + "include/pcl/${SUBSYS_NAME}/impl/grid_minimum.hpp" ) set(LIB_NAME "pcl_${SUBSYS_NAME}") diff --git a/filters/include/pcl/filters/grid_minimum.h b/filters/include/pcl/filters/grid_minimum.h new file mode 100644 index 00000000000..b4f9f894ce3 --- /dev/null +++ b/filters/include/pcl/filters/grid_minimum.h @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_VOXEL_GRID_MINIMUM_H_ +#define PCL_FILTERS_VOXEL_GRID_MINIMUM_H_ + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief GridMinimum assembles a local 2D grid over a given PointCloud, and downsamples the data. + * + * The GridMinimum class creates a *2D grid* over the input point cloud + * data. Then, in each *cell* (i.e., 2D grid element), all the points + * present will be *downsampled* with the minimum z value. This grid minimum + * can be useful in a number of topographic processing tasks such as crudely + * estimating ground returns, especially under foliage. + * + * \author Bradley J Chambers + * \ingroup filters + */ + template + class GridMinimum: public FilterIndices + { + protected: + using Filter::filter_name_; + using Filter::getClassName; + using Filter::input_; + using Filter::indices_; + + typedef typename FilterIndices::PointCloud PointCloud; + + public: + /** \brief Empty constructor. */ + GridMinimum () : + leaf_size_ (Eigen::Vector4f::Zero ()), + inverse_leaf_size_ (Eigen::Array4f::Zero ()), + min_b_ (Eigen::Vector4i::Zero ()), + max_b_ (Eigen::Vector4i::Zero ()), + div_b_ (Eigen::Vector4i::Zero ()), + divb_mul_ (Eigen::Vector4i::Zero ()), + min_points_per_grid_ (0) + { + filter_name_ = "GridMinimum"; + } + + /** \brief Destructor. */ + virtual ~GridMinimum () + { + } + + /** \brief Set the grid leaf size. + * \param[in] leaf_size the grid leaf size + */ + inline void + setLeafSize (const Eigen::Vector4f &leaf_size) + { + leaf_size_ = leaf_size; + // Avoid division errors + if (leaf_size_[2] == 0) + leaf_size_[2] = 1; + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + } + + /** \brief Set the grid leaf size. + * \param[in] lx the leaf size for X + * \param[in] ly the leaf size for Y + */ + inline void + setLeafSize (float lx, float ly) + { + leaf_size_[0] = lx; leaf_size_[1] = ly; + // Avoid division errors + if (leaf_size_[2] == 0) + leaf_size_[2] = 1; + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + } + + /** \brief Get the grid leaf size. */ + inline Eigen::Vector3f + getLeafSize () { return (leaf_size_.head<3> ()); } + + /** \brief Set the minimum number of points required for a grid cell to be used. + * \param[in] min_points_per_voxel the minimum number of points for required for a cell to be used + */ + inline void + setMinimumPointsNumberPerGrid (unsigned int min_points_per_grid) { min_points_per_grid_ = min_points_per_grid; } + + /** \brief Return the minimum number of points required for a grid cell to be used. + */ + inline unsigned int + getMinimumPointsNumberPerGrid () { return min_points_per_grid_; } + + /** \brief Get the minimum coordinates of the bounding box (after + * filtering is performed). + */ + inline Eigen::Vector3i + getMinBoxCoordinates () { return (min_b_.head<3> ()); } + + /** \brief Get the maximum coordinates of the bounding box (after + * filtering is performed). + */ + inline Eigen::Vector3i + getMaxBoxCoordinates () { return (max_b_.head<3> ()); } + + /** \brief Get the number of divisions along all 3 axes (after filtering + * is performed). + */ + inline Eigen::Vector3i + getNrDivisions () { return (div_b_.head<3> ()); } + + /** \brief Get the multipliers to be applied to the grid coordinates in + * order to find the centroid index (after filtering is performed). + */ + inline Eigen::Vector3i + getDivisionMultiplier () { return (divb_mul_.head<3> ()); } + + protected: + /** \brief The size of a leaf. */ + Eigen::Vector4f leaf_size_; + + /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ + Eigen::Array4f inverse_leaf_size_; + + /** \brief Minimum number of points per grid cell for the minimum to be computed */ + unsigned int min_points_per_grid_; + + /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */ + Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; + + /** \brief Downsample a Point Cloud using a 2D grid approach + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output); + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_FILTERS_VOXEL_GRID_MINIMUM_H_ + diff --git a/filters/include/pcl/filters/impl/grid_minimum.hpp b/filters/include/pcl/filters/impl/grid_minimum.hpp new file mode 100644 index 00000000000..15c9b089459 --- /dev/null +++ b/filters/include/pcl/filters/impl/grid_minimum.hpp @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ +#define PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ + +#include +#include +#include + +struct cloud_point_index_idx +{ + unsigned int idx; + unsigned int cloud_point_index; + + cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} + bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); } +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridMinimum::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + std::vector indices; + + output.is_dense = true; + applyFilterIndices (indices); + pcl::copyPointCloud (*input_, indices, output); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridMinimum::applyFilterIndices (std::vector &indices) +{ + indices.resize (indices_->size ()); + int oii = 0; + + // Get the minimum and maximum dimensions + Eigen::Vector4f min_p, max_p; + getMinMax3D (*input_, *indices_, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + int64_t dx = static_cast ((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; + int64_t dy = static_cast ((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; + + if ((dx*dy) > static_cast (std::numeric_limits::max ())) + { + PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName ().c_str ()); + return; + } + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast (floor (min_p[0] * inverse_leaf_size_[0])); + max_b_[0] = static_cast (floor (max_p[0] * inverse_leaf_size_[0])); + min_b_[1] = static_cast (floor (min_p[1] * inverse_leaf_size_[1])); + max_b_[1] = static_cast (floor (max_p[1] * inverse_leaf_size_[1])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); + div_b_[3] = 0; + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i (1, div_b_[0], 0, 0); + + std::vector index_vector; + index_vector.reserve (indices_->size ()); + + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!pcl_isfinite (input_->points[*it].x) || + !pcl_isfinite (input_->points[*it].y) || + !pcl_isfinite (input_->points[*it].z)) + continue; + + int ijk0 = static_cast (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); + int ijk1 = static_cast (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1]; + index_vector.push_back (cloud_point_index_idx (static_cast (idx), *it)); + } + + // Second pass: sort the index_vector vector using value representing target cell as index + // in effect all points belonging to the same output cell will be next to each other + std::sort (index_vector.begin (), index_vector.end (), std::less ()); + + // Third pass: count output cells + // we need to skip all the same, adjacenent idx values + unsigned int total = 0; + unsigned int index = 0; + + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector > first_and_last_indices_vector; + + // Worst case size + first_and_last_indices_vector.reserve (index_vector.size ()); + while (index < index_vector.size ()) + { + unsigned int i = index + 1; + while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) + ++i; + if (i - index >= min_points_per_grid_) + { + ++total; + first_and_last_indices_vector.push_back (std::pair (index, i)); + } + index = i; + } + + // Fourth pass: compute centroids, insert them into their final position + indices.resize (total); + + index = 0; + + for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp) + { + // calculate centroid - sum values from all input points, that have the same idx value in index_vector array + unsigned int first_index = first_and_last_indices_vector[cp].first; + unsigned int last_index = first_and_last_indices_vector[cp].second; + unsigned int min_index = index_vector[first_index].cloud_point_index; + float min_z = input_->points[index_vector[first_index].cloud_point_index].z; + + for (unsigned int i = first_index + 1; i < last_index; ++i) + { + if (input_->points[index_vector[i].cloud_point_index].z < min_z) + { + min_z = input_->points[index_vector[i].cloud_point_index].z; + min_index = index_vector[i].cloud_point_index; + } + } + + indices[index] = min_index; + + ++index; + } + + oii = indices.size (); + + // Resize the output arrays + indices.resize (oii); +} + +#define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum; + +#endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ + diff --git a/filters/src/grid_minimum.cpp b/filters/src/grid_minimum.cpp new file mode 100644 index 00000000000..6eb88a6d07a --- /dev/null +++ b/filters/src/grid_minimum.cpp @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(GridMinimum, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE + diff --git a/test/filters/CMakeLists.txt b/test/filters/CMakeLists.txt index b945df82ace..4d414cd0476 100644 --- a/test/filters/CMakeLists.txt +++ b/test/filters/CMakeLists.txt @@ -15,5 +15,8 @@ PCL_ADD_TEST(filters_bilateral test_filters_bilateral LINK_WITH pcl_gtest pcl_filters pcl_io pcl_kdtree ARGUMENTS "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd") +PCL_ADD_TEST(filters_grid_minimum test_filters_grid_minimum + FILES test_grid_minimum.cpp + LINK_WITH pcl_gtest pcl_common pcl_filters) diff --git a/test/filters/test_grid_minimum.cpp b/test/filters/test_grid_minimum.cpp new file mode 100644 index 00000000000..665ecc133d3 --- /dev/null +++ b/test/filters/test_grid_minimum.cpp @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include +#include +#include + +using namespace pcl; + +PointCloud cloud; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (Grid, Minimum) +{ + PointCloud cloud_in, cloud_out; + + cloud_in.height = 1; + cloud_in.width = 2; + cloud_in.is_dense = true; + cloud_in.resize (2); + + cloud_in[0].x = 0; cloud_in[0].y = 0; cloud_in[0].z = 0.25; + cloud_in[1].x = 0.5; cloud_in[1].y = 0.5; cloud_in[1].z = 1; + + GridMinimum gm; + gm.setInputCloud (cloud_in.makeShared ()); + gm.setLeafSize (1.0f, 1.0f); + gm.filter (cloud_out); + + EXPECT_EQ (cloud_out[0].z, 0.25f); + EXPECT_EQ (cloud_out.size(), 1); +} + +/* ---[ */ +int +main (int argc, char** argv) +{ + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */ + From 5e825272e9b354f1adff942b07879f2d179bec10 Mon Sep 17 00:00:00 2001 From: Bradley J Chambers Date: Thu, 20 Feb 2014 05:38:06 +0000 Subject: [PATCH 2/2] Update: avoid error with redefinition of struct by renaming it --- filters/include/pcl/filters/impl/grid_minimum.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/filters/include/pcl/filters/impl/grid_minimum.hpp b/filters/include/pcl/filters/impl/grid_minimum.hpp index 15c9b089459..04157b5dab6 100644 --- a/filters/include/pcl/filters/impl/grid_minimum.hpp +++ b/filters/include/pcl/filters/impl/grid_minimum.hpp @@ -46,13 +46,13 @@ #include #include -struct cloud_point_index_idx +struct point_index_idx { unsigned int idx; unsigned int cloud_point_index; - cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} - bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); } + point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} + bool operator < (const point_index_idx &p) const { return (idx < p.idx); } }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -109,7 +109,7 @@ pcl::GridMinimum::applyFilterIndices (std::vector &indices) // Set up the division multiplier divb_mul_ = Eigen::Vector4i (1, div_b_[0], 0, 0); - std::vector index_vector; + std::vector index_vector; index_vector.reserve (indices_->size ()); // First pass: go over all points and insert them into the index_vector vector @@ -129,12 +129,12 @@ pcl::GridMinimum::applyFilterIndices (std::vector &indices) // Compute the centroid leaf index int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1]; - index_vector.push_back (cloud_point_index_idx (static_cast (idx), *it)); + index_vector.push_back (point_index_idx (static_cast (idx), *it)); } // Second pass: sort the index_vector vector using value representing target cell as index // in effect all points belonging to the same output cell will be next to each other - std::sort (index_vector.begin (), index_vector.end (), std::less ()); + std::sort (index_vector.begin (), index_vector.end (), std::less ()); // Third pass: count output cells // we need to skip all the same, adjacenent idx values