diff --git a/sensing/autoware_pcl_extensions/CMakeLists.txt b/sensing/autoware_pcl_extensions/CMakeLists.txt new file mode 100644 index 0000000000..0b5ae0b755 --- /dev/null +++ b/sensing/autoware_pcl_extensions/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_pcl_extensions) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +ament_auto_add_library(autoware_pcl_extensions SHARED + src/voxel_grid_nearest_centroid.cpp +) + +target_link_libraries(autoware_pcl_extensions ${PCL_LIBRARIES}) + +ament_auto_package() diff --git a/sensing/autoware_pcl_extensions/README.md b/sensing/autoware_pcl_extensions/README.md new file mode 100644 index 0000000000..5dca2d0df4 --- /dev/null +++ b/sensing/autoware_pcl_extensions/README.md @@ -0,0 +1,56 @@ +# autoware_pcl_extensions + +## Overview + +The `autoware_pcl_extensions` is a pcl extension library. The voxel grid filter in this package works with a different algorithm than the original one. + +## Design + +Inner-workings / Algorithms + +### Original Algorithm [1] + +1. create a 3D voxel grid over the input pointcloud data +2. calculate centroid in each voxel +3. all the points are approximated with their centroid + +### Extended Algorithm + +1. create a 3D voxel grid over the input pointcloud data +2. calculate centroid in each voxel +3. **all the points are approximated with the closest point to their centroid** + +## Inputs / Outputs + +input and output are pointcloud with the datatype `pcl::PointCloud` + +## Parameters + +`leaf_size`: it represents the scale of each cell in 3 dimension. + +It can be set by calling this function, where the scale of the cell in the axis of x, y and z are set respectively + +```cpp +filter.setLeafSize(voxel_size_x_, voxel_size_y_, voxel_size_z_); +``` + +## Usage + +include the header file + +```cpp +#include +``` + +init the filter with input pointcloud, set leaf size and get output result + +```cpp +pcl::VoxelGridNearestCentroid filter; +filter.setInputCloud(pcl_input); +filter.setLeafSize(voxel_size_x_, voxel_size_y_, voxel_size_z_); +filter.filter(*pcl_output); +``` + +## (Optional) References/External links + +[1] diff --git a/sensing/autoware_pcl_extensions/include/autoware/pcl_extensions/voxel_grid_nearest_centroid.hpp b/sensing/autoware_pcl_extensions/include/autoware/pcl_extensions/voxel_grid_nearest_centroid.hpp new file mode 100644 index 0000000000..282da3716c --- /dev/null +++ b/sensing/autoware_pcl_extensions/include/autoware/pcl_extensions/voxel_grid_nearest_centroid.hpp @@ -0,0 +1,544 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, 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. + * + */ + +#ifndef AUTOWARE__PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ +#define AUTOWARE__PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ + +#include + +#if PCL_VERSION < PCL_VERSION_CALC(1, 12, 0) +#include +#endif + +#include +#include +#include + +#include +#include + +namespace pcl +{ +/** \brief A searchable voxel strucure containing the mean and covariance of the data. + * \note For more information please see + * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — + * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. + * PhD thesis, Orebro University. Orebro Studies in Technology 36 + * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) + */ +template +class VoxelGridNearestCentroid : public VoxelGrid +{ +protected: + using VoxelGrid::filter_name_; + using VoxelGrid::getClassName; + using VoxelGrid::input_; + using VoxelGrid::indices_; + using VoxelGrid::filter_limit_negative_; + using VoxelGrid::filter_limit_min_; + using VoxelGrid::filter_limit_max_; + using VoxelGrid::filter_field_name_; + + using VoxelGrid::downsample_all_data_; + using VoxelGrid::leaf_layout_; + using VoxelGrid::save_leaf_layout_; + using VoxelGrid::leaf_size_; + using VoxelGrid::min_b_; + using VoxelGrid::max_b_; + using VoxelGrid::inverse_leaf_size_; + using VoxelGrid::div_b_; + // cspell: ignore divb + using VoxelGrid::divb_mul_; + + typedef typename pcl::traits::fieldList::type FieldList; + typedef typename Filter::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + +public: + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; + + /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf. + * Inverse covariance, eigen vectors and eigen values are precomputed. */ + struct Leaf + { + /** \brief Constructor. + * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref eigen_values_ to 0 and \ref cov_ and + * \ref eigen_vectors_ to the identity matrix + */ + Leaf() + : nr_points(0), + // mean_ (Eigen::Vector3d::Zero ()), + centroid() + // cov_ (Eigen::Matrix3d::Identity ()), + // icov_ (Eigen::Matrix3d::Zero ()), + // eigen_vectors_ (Eigen::Matrix3d::Identity ()), + // eigen_values_ (Eigen::Vector3d::Zero ()) + { + } + + /** \brief Get the voxel covariance. + * \return covariance matrix + */ + // Eigen::Matrix3d + // getCov () const + // { + // return (cov_); + // } + + /** \brief Get the inverse of the voxel covariance. + * \return inverse covariance matrix + */ + // Eigen::Matrix3d + // getInverseCov () const + // { + // return (icov_); + // } + + /** \brief Get the voxel centroid. + * \return centroid + */ + // Eigen::Vector3d + // getMean () const + // { + // return (mean_); + // } + + /** \brief Get the eigen vectors of the voxel covariance. + * \note Order corresponds with \ref getEigenValues + * \return matrix whose columns contain eigen vectors + */ + // Eigen::Matrix3d + // getEigenVectors () const + // { + // return (eigen_vectors_); + // } + + /** \brief Get the eigen values of the voxel covariance. + * \note Order corresponds with \ref getEigenVectors + * \return vector of eigen values + */ + // Eigen::Vector3d + // getEigenValues () const + // { + // return (eigen_values_); + // } + + /** \brief Get the number of points contained by this voxel. + * \return number of points + */ + int getPointCount() const { return nr_points; } + + /** \brief Number of points contained by voxel */ + int nr_points; + + /** \brief 3D voxel centroid */ + // Eigen::Vector3d mean_; + + /** \brief Nd voxel centroid + * \note Differs from \ref mean_ when color data is used + */ + Eigen::VectorXf centroid; + + /** \brief Voxel covariance matrix */ + // Eigen::Matrix3d cov_; + + /** \brief Inverse of voxel covariance matrix */ + // Eigen::Matrix3d icov_; + + /** \brief Eigen vectors of voxel covariance matrix */ + // Eigen::Matrix3d eigen_vectors_; + + /** \brief Eigen values of voxel covariance matrix */ + // Eigen::Vector3d eigen_values_; + + PointCloud points; + }; + + /** \brief Pointer to VoxelGridNearestCentroid leaf structure */ + typedef Leaf * LeafPtr; + + /** \brief Const pointer to VoxelGridNearestCentroid leaf structure */ + typedef const Leaf * LeafConstPtr; + +public: + /** \brief Constructor. + * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. + */ + VoxelGridNearestCentroid() + : searchable_(true), + // min_points_per_voxel_ (6), + min_points_per_voxel_(1), + // min_cov_eigenvalue_mult_ (0.01), + leaves_(), + voxel_centroids_(), + voxel_centroids_leaf_indices_(), + kdtree_() + { + downsample_all_data_ = false; + save_leaf_layout_ = false; + leaf_size_.setZero(); + min_b_.setZero(); + max_b_.setZero(); + filter_name_ = "VoxelGridNearestCentroid"; + } + + /** \brief Set the minimum number of points required for a cell to be used + * (must be 3 or greater for covariance calculation). + * \param[in] min_points_per_voxel the minimum number of points for required + * for a voxel to be used + */ + inline void setMinPointPerVoxel(int min_points_per_voxel) + { + // if(min_points_per_voxel > 2) + if (min_points_per_voxel > 1) { + min_points_per_voxel_ = min_points_per_voxel; + } else { + // PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per + // Voxel to 3 ", this->getClassName ().c_str ()); + // min_points_per_voxel_ = 3; + PCL_WARN( + "%s: Covariance calculation requires at least 1 points, setting Min Point per Voxel to 3 ", + this->getClassName().c_str()); + min_points_per_voxel_ = 1; + } + } + + /** \brief Get the minimum number of points required for a cell to be used. + * \return the minimum number of points for required for a voxel to be used + */ + inline int getMinPointPerVoxel() { return min_points_per_voxel_; } + + /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance + * matrices. \param[in] min_cov_eigenvalue_mult the minimum allowable ratio between eigenvalues + */ + // inline void + // setCovEigValueInflationRatio (double min_cov_eigenvalue_mult) + // { + // min_cov_eigenvalue_mult_ = min_cov_eigenvalue_mult; + // } + + /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance + * matrices. \return the minimum allowable ratio between eigenvalues + */ + // inline double + // getCovEigValueInflationRatio () + // { + // return min_cov_eigenvalue_mult_; + // } + + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient number of + * points \param[in] searchable flag if voxel structure is searchable, if true then kdtree is + * built + */ + inline void filter(PointCloud & output, bool searchable = false) + { + searchable_ = searchable; + applyFilter(output); + + voxel_centroids_ = PointCloudPtr(new PointCloud(output)); + + if (searchable_ && voxel_centroids_->size() > 0) { + // Initiates kdtree of the centroids of voxels containing a sufficient number of points + kdtree_.setInputCloud(voxel_centroids_); + } + } + + /** \brief Initializes voxel structure. + * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built + */ + inline void filter(bool searchable = false) + { + searchable_ = searchable; + voxel_centroids_ = PointCloudPtr(new PointCloud); + applyFilter(*voxel_centroids_); + + if (searchable_ && voxel_centroids_->size() > 0) { + // Initiates kdtree of the centroids of voxels containing a sufficient number of points + kdtree_.setInputCloud(voxel_centroids_); + } + } + + /** \brief Get the voxel containing point p. + * \param[in] index the index of the leaf structure node + * \return const pointer to leaf structure + */ + inline LeafConstPtr getLeaf(int index) + { + typename std::map::iterator leaf_iter = leaves_.find(index); + if (leaf_iter != leaves_.end()) { + LeafConstPtr ret(&(leaf_iter->second)); + return ret; + } else { + return NULL; + } + } + + /** \brief Get the voxel containing point p. + * \param[in] p the point to get the leaf structure at + * \return const pointer to leaf structure + */ + inline LeafConstPtr getLeaf(PointT & p) + { + // Generate index associated with p + int ijk0 = static_cast(floor(p.x * inverse_leaf_size_[0]) - min_b_[0]); + int ijk1 = static_cast(floor(p.y * inverse_leaf_size_[1]) - min_b_[1]); + int ijk2 = static_cast(floor(p.z * inverse_leaf_size_[2]) - min_b_[2]); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + // Find leaf associated with index + typename std::map::iterator leaf_iter = leaves_.find(idx); + if (leaf_iter != leaves_.end()) { + // If such a leaf exists return the pointer to the leaf structure + LeafConstPtr ret(&(leaf_iter->second)); + return ret; + } else { + return NULL; + } + } + + /** \brief Get the voxel containing point p. + * \param[in] p the point to get the leaf structure at + * \return const pointer to leaf structure + */ + inline LeafConstPtr getLeaf(Eigen::Vector3f & p) + { + // Generate index associated with p + int ijk0 = static_cast(floor(p[0] * inverse_leaf_size_[0]) - min_b_[0]); + int ijk1 = static_cast(floor(p[1] * inverse_leaf_size_[1]) - min_b_[1]); + int ijk2 = static_cast(floor(p[2] * inverse_leaf_size_[2]) - min_b_[2]); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + // Find leaf associated with index + typename std::map::iterator leaf_iter = leaves_.find(idx); + if (leaf_iter != leaves_.end()) { + // If such a leaf exists return the pointer to the leaf structure + LeafConstPtr ret(&(leaf_iter->second)); + return ret; + } else { + return NULL; + } + } + + /** \brief Get the voxels surrounding point p, not including the voxel containing point p. + * \note Only voxels containing a sufficient number of points are used (slower than radius search + * in practice). \param[in] reference_point the point to get the leaf structure at \param[out] + * neighbors \return number of neighbors found + */ + // int + // getNeighborhoodAtPoint (const PointT& reference_point, std::vector &neighbors); + + /** \brief Get the leaf structure map + * \return a map containing all leaves + */ + inline const std::map & getLeaves() { return leaves_; } + + /** \brief Get a pointcloud containing the voxel centroids + * \note Only voxels containing a sufficient number of points are used. + * \return a map containing all leaves + */ + inline PointCloudPtr getCentroids() { return voxel_centroids_; } + + /** \brief Get a cloud to visualize each voxels normal distribution. + * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel + */ + // void + // getDisplayCloud (pcl::PointCloud& cell_cloud); + + /** \brief Search for the k-nearest occupied voxels for the given query point. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \return number of neighbors found + */ + int nearestKSearch( + const PointT & point, int k, std::vector & k_leaves, + std::vector & k_sqr_distances) + { + k_leaves.clear(); + + // Check if kdtree has been built + if (!searchable_) { + PCL_WARN("%s: Not Searchable", this->getClassName().c_str()); + return 0; + } + + // Find k-nearest neighbors in the occupied voxel centroid cloud + std::vector k_indices; + k = kdtree_.nearestKSearch(point, k, k_indices, k_sqr_distances); + + // Find leaves corresponding to neighbors + k_leaves.reserve(k); + for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) { + k_leaves.push_back(&leaves_[voxel_centroids_leaf_indices_[*iter]]); + } + return k; + } + + /** \brief Search for the k-nearest occupied voxels for the given query point. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index the index + * \param[in] k the number of neighbors to search for + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \return number of neighbors found + */ + inline int nearestKSearch( + const PointCloud & cloud, int index, int k, std::vector & k_leaves, + std::vector & k_sqr_distances) + { + if (index >= static_cast(cloud.points.size()) || index < 0) { + return 0; + } + return nearestKSearch(cloud.points[index], k, k_leaves, k_sqr_distances); + } + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + int radiusSearch( + const PointT & point, double radius, std::vector & k_leaves, + std::vector & k_sqr_distances, unsigned int max_nn = 0) + { + k_leaves.clear(); + + // Check if kdtree has been built + if (!searchable_) { + PCL_WARN("%s: Not Searchable", this->getClassName().c_str()); + return 0; + } + + // Find neighbors within radius in the occupied voxel centroid cloud + std::vector k_indices; + int k = kdtree_.radiusSearch(point, radius, k_indices, k_sqr_distances, max_nn); + + // Find leaves corresponding to neighbors + k_leaves.reserve(k); + for (std::vector::iterator iter = k_indices.begin(); iter != k_indices.end(); iter++) { + k_leaves.push_back(&leaves_[voxel_centroids_leaf_indices_[*iter]]); + } + return k; + } + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + inline int radiusSearch( + const PointCloud & cloud, int index, double radius, std::vector & k_leaves, + std::vector & k_sqr_distances, unsigned int max_nn = 0) + { + if (index >= static_cast(cloud.points.size()) || index < 0) { + return 0; + } + return radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn); + } + +protected: + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient + * number of points + */ + void applyFilter(PointCloud & output); + + /** \brief Flag to determine if voxel structure is searchable. */ + bool searchable_; + + /** \brief Minimum points contained with in a voxel to allow it to be useable. */ + int min_points_per_voxel_; + + /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance + * matrices. */ + // double min_cov_eigenvalue_mult_; + + /** \brief Voxel structure containing all leaf nodes (includes voxels with less than + * a sufficient number of points). */ + std::map leaves_; + + /** \brief Point cloud containing centroids of voxels containing atleast + * minimum number of points. */ + PointCloudPtr voxel_centroids_; + + /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ + * (used for searching). */ + std::vector voxel_centroids_leaf_indices_; + + /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ + KdTreeFLANN kdtree_; +}; +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +// #include +#include +#endif + +#endif // AUTOWARE__PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ diff --git a/sensing/autoware_pcl_extensions/include/autoware/pcl_extensions/voxel_grid_nearest_centroid_impl.hpp b/sensing/autoware_pcl_extensions/include/autoware/pcl_extensions/voxel_grid_nearest_centroid_impl.hpp new file mode 100644 index 0000000000..927df54a3f --- /dev/null +++ b/sensing/autoware_pcl_extensions/include/autoware/pcl_extensions/voxel_grid_nearest_centroid_impl.hpp @@ -0,0 +1,350 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, 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. + * + */ + +#ifndef AUTOWARE__PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_IMPL_HPP_ +#define AUTOWARE__PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_IMPL_HPP_ + +#include "autoware/pcl_extensions/voxel_grid_nearest_centroid.hpp" + +#include +#include + +#include +#include + +#if PCL_VERSION < PCL_VERSION_CALC(1, 12, 0) +#include +#endif + +#include + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::VoxelGridNearestCentroid::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; + } + + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // downsampling breaks the organized structure + output.is_dense = true; // we filter out invalid points + output.points.clear(); + + Eigen::Vector4f min_p, max_p; + // Get the minimum and maximum dimensions + if (!filter_field_name_.empty()) { // If we don't want to process the entire cloud... + getMinMax3D( + input_, filter_field_name_, static_cast(filter_limit_min_), + static_cast(filter_limit_max_), min_p, max_p, filter_limit_negative_); + } else { + getMinMax3D(*input_, min_p, max_p); + } + + // Check that the leaf size is not too small, given the size of the data + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; + + if ((dx * dy * dz) > std::numeric_limits::max()) { + PCL_WARN( + "[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would " + "overflow.", // NOLINT + getClassName().c_str()); + output.clear(); + 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])); + min_b_[2] = static_cast(floor(min_p[2] * inverse_leaf_size_[2])); + max_b_[2] = static_cast(floor(max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones(); + div_b_[3] = 0; + + // Clear the leaves + leaves_.clear(); + + // cspell: ignore divb + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0); + + int centroid_size = 4; + + if (downsample_all_data_) { + centroid_size = boost::mpl::size::value; + } + + // ---[ RGB special case + std::vector fields; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex("rgb", fields); + if (rgba_index == -1) { + rgba_index = pcl::getFieldIndex("rgba", fields); + } + if (rgba_index >= 0) { + rgba_index = fields[rgba_index].offset; + centroid_size += 3; + } + + // If we don't want to process the entire cloud, but rather filter points far + // away from the viewpoint first... + if (!filter_field_name_.empty()) { + // Get the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); + if (distance_idx == -1) { + PCL_WARN( + "[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName().c_str(), + distance_idx); + } + + // First pass: go over all points and insert them into the right leaf + for (size_t cp = 0; cp < input_->points.size(); ++cp) { + if (!input_->is_dense) { + // Check if the point is invalid + if ( + !std::isfinite(input_->points[cp].x) || !std::isfinite(input_->points[cp].y) || + !std::isfinite(input_->points[cp].z)) { + continue; + } + } + + // Get the distance value + const std::uint8_t * pt_data = reinterpret_cast(&input_->points[cp]); + float distance_value = 0; + memcpy(&distance_value, pt_data + fields[distance_idx].offset, sizeof(float)); + + if (filter_limit_negative_) { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) { + continue; + } + } else { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) { + continue; + } + } + + int ijk0 = static_cast( + floor(input_->points[cp].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); + int ijk1 = static_cast( + floor(input_->points[cp].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); + int ijk2 = static_cast( + floor(input_->points[cp].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + Leaf & leaf = leaves_[idx]; + if (leaf.nr_points == 0) { + leaf.centroid.resize(centroid_size); + leaf.centroid.setZero(); + } + + // Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); + + // Do we need to process all the fields? + if (!downsample_all_data_) { + Eigen::Vector4f pt(input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); + leaf.centroid.template head<4>() += pt; + } else { + // Copy all the fields + Eigen::VectorXf centroid = Eigen::VectorXf::Zero(centroid_size); + // ---[ RGB special case + if (rgba_index >= 0) { + // fill r/g/b data + int rgb; + memcpy( + &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); + centroid[centroid_size - 3] = static_cast((rgb >> 16) & 0x0000ff); + centroid[centroid_size - 2] = static_cast((rgb >> 8) & 0x0000ff); + centroid[centroid_size - 1] = static_cast((rgb) & 0x0000ff); + } + pcl::for_each_type( + NdCopyPointEigenFunctor(input_->points[cp], centroid)); + leaf.centroid += centroid; + } + ++leaf.nr_points; + } + } else { // No distance filtering, process all data + // First pass: go over all points and insert them into the right leaf + for (size_t cp = 0; cp < input_->points.size(); ++cp) { + if (!input_->is_dense) { + // Check if the point is invalid + if ( + !std::isfinite(input_->points[cp].x) || !std::isfinite(input_->points[cp].y) || + !std::isfinite(input_->points[cp].z)) { + continue; + } + } + + int ijk0 = static_cast( + floor(input_->points[cp].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); + int ijk1 = static_cast( + floor(input_->points[cp].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); + int ijk2 = static_cast( + floor(input_->points[cp].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + Leaf & leaf = leaves_[idx]; + if (leaf.nr_points == 0) { + leaf.centroid.resize(centroid_size); + leaf.centroid.setZero(); + } + + // Do we need to process all the fields? + if (!downsample_all_data_) { + Eigen::Vector4f pt(input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); + leaf.centroid.template head<4>() += pt; + } else { + // Copy all the fields + Eigen::VectorXf centroid = Eigen::VectorXf::Zero(centroid_size); + // ---[ RGB special case + if (rgba_index >= 0) { + // Fill r/g/b data, assuming that the order is BGRA + int rgb; + memcpy( + &rgb, reinterpret_cast(&input_->points[cp]) + rgba_index, sizeof(int)); + centroid[centroid_size - 3] = static_cast((rgb >> 16) & 0x0000ff); + centroid[centroid_size - 2] = static_cast((rgb >> 8) & 0x0000ff); + centroid[centroid_size - 1] = static_cast((rgb) & 0x0000ff); + } + pcl::for_each_type( + NdCopyPointEigenFunctor(input_->points[cp], centroid)); + leaf.centroid += centroid; + } + leaf.points.push_back(input_->points[cp]); + ++leaf.nr_points; + } + } + + // Second pass: go over all leaves and compute centroids and covariance matrices + output.points.reserve(leaves_.size()); + if (searchable_) { + voxel_centroids_leaf_indices_.reserve(leaves_.size()); + } + int cp = 0; + if (save_leaf_layout_) { + leaf_layout_.resize(div_b_[0] * div_b_[1] * div_b_[2], -1); + } + + // Eigen values and vectors calculated to prevent near singular matrices + // Eigen::SelfAdjointEigenSolver eigensolver; + // Eigen::Matrix3d eigen_val; + // Eigen::Vector3d pt_sum; + + // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the + // max eigen value. + // double min_cov_eigenvalue; + + for (typename std::map::iterator it = leaves_.begin(); it != leaves_.end(); ++it) { + // Normalize the centroid + Leaf & leaf = it->second; + + // Normalize the centroid + leaf.centroid /= static_cast(leaf.nr_points); + // Point sum used for single pass covariance calculation + // pt_sum = leaf.mean_; + // Normalize mean + // leaf.mean_ /= leaf.nr_points; + + // If the voxel contains sufficient points, its covariance is calculated and is added to the + // voxel centroids and output clouds. + // Points with less than the minimum points will have a can not be accurately approximated + // using a normal distribution. + if (leaf.nr_points >= min_points_per_voxel_) { + if (save_leaf_layout_) { + leaf_layout_[it->first] = cp++; + } + + output.push_back(PointT()); + + const auto & centroid = leaf.centroid; + const auto squared_distances = + leaf.points | ranges::views::transform([¢roid](const auto & p) { + return (p.x - centroid[0]) * (p.x - centroid[0]) + + (p.y - centroid[1]) * (p.y - centroid[1]) + + (p.z - centroid[2]) * (p.z - centroid[2]); + }); + const auto min_itr = ranges::min_element(squared_distances); + const auto min_idx = ranges::distance(squared_distances.begin(), min_itr); + output.points.back() = leaf.points.at(min_idx); + + // Stores the voxel indices for fast access searching + if (searchable_) { + voxel_centroids_leaf_indices_.push_back(static_cast(it->first)); + } + } + } + output.width = static_cast(output.points.size()); +} + +#define PCL_INSTANTIATE_VoxelGridNearestCentroid(T) \ + template class PCL_EXPORTS pcl::VoxelGridNearestCentroid; + +#endif // AUTOWARE__PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_IMPL_HPP_ diff --git a/sensing/autoware_pcl_extensions/package.xml b/sensing/autoware_pcl_extensions/package.xml new file mode 100644 index 0000000000..2eb1fab8c0 --- /dev/null +++ b/sensing/autoware_pcl_extensions/package.xml @@ -0,0 +1,33 @@ + + + + autoware_pcl_extensions + 0.0.0 + The autoware_pcl_extensions package + Xingang Liu + Ryu Yamamoto + Kenzo Lobos Tsunekawa + David Wong + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + eigen3_cmake_module + + eigen + range-v3 + + eigen + + libpcl-all-dev + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/autoware_pcl_extensions/src/voxel_grid_nearest_centroid.cpp b/sensing/autoware_pcl_extensions/src/voxel_grid_nearest_centroid.cpp new file mode 100644 index 0000000000..e715281c8a --- /dev/null +++ b/sensing/autoware_pcl_extensions/src/voxel_grid_nearest_centroid.cpp @@ -0,0 +1,64 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, 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. + * + */ + +#include "autoware/pcl_extensions/voxel_grid_nearest_centroid_impl.hpp" + +#include + +#ifndef PCL_NO_PRECOMPILE +#include + +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(VoxelGridNearestCentroid, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE