From 31e380328b4bb46c5f0639536264e300cf648f00 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Fri, 1 Mar 2024 18:42:40 +0900 Subject: [PATCH] Refactored MultiVoxelGridCovariance and MultiGridNormalDistributionsTransform Signed-off-by: Shintaro Sakoda --- .../multi_voxel_grid_covariance_omp.h | 128 +++++++---------- .../multi_voxel_grid_covariance_omp_impl.hpp | 130 ++++++++++++++---- include/multigrid_pclomp/multigrid_ndt_omp.h | 2 +- .../multigrid_ndt_omp_impl.hpp | 8 +- 4 files changed, 164 insertions(+), 104 deletions(-) diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h index 61fe3373..33bfaee9 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h @@ -95,20 +95,52 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { public: #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) - typedef pcl::shared_ptr > Ptr; - typedef pcl::shared_ptr > ConstPtr; + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; #else - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> ConstPtr; #endif /** \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 evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix + * Sets \ref nr_points_, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix */ - Leaf() : nr_points(0), mean_(Eigen::Vector3d::Zero()), centroid(), cov_(Eigen::Matrix3d::Identity()), icov_(Eigen::Matrix3d::Zero()), evecs_(Eigen::Matrix3d::Identity()), evals_(Eigen::Vector3d::Zero()) {} + Leaf() : nr_points_(0), mean_(Eigen::Vector3d::Zero()), centroid_(), cov_(Eigen::Matrix3d::Identity()), icov_(Eigen::Matrix3d::Zero()), evecs_(Eigen::Matrix3d::Identity()), evals_(Eigen::Vector3d::Zero()) {} + + Leaf(const Leaf &other) : mean_(other.mean_), centroid_(other.centroid_), cov_(other.cov_), icov_(other.icov_), evecs_(other.evecs_), evals_(other.evals_) { + nr_points_ = other.nr_points_; + } + + Leaf(Leaf &&other) : mean_(std::move(other.mean_)), centroid_(std::move(other.centroid_)), cov_(std::move(other.cov_)), icov_(std::move(other.icov_)), evecs_(std::move(other.evecs_)), evals_(std::move(other.evals_)) { + nr_points_ = other.nr_points_; + } + + Leaf &operator=(const Leaf &other) { + mean_ = other.mean_; + centroid_ = other.centroid_; + cov_ = other.cov_; + icov_ = other.icov_; + evecs_ = other.evecs_; + evals_ = other.evals_; + nr_points_ = other.nr_points_; + + return *this; + } + + Leaf &operator=(Leaf &&other) { + mean_ = std::move(other.mean_); + centroid_ = std::move(other.centroid_); + cov_ = std::move(other.cov_); + icov_ = std::move(other.icov_); + evecs_ = std::move(other.evecs_); + evals_ = std::move(other.evals_); + nr_points_ = other.nr_points_; + + return *this; + } /** \brief Get the voxel covariance. * \return covariance matrix @@ -151,11 +183,11 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { * \return number of points */ int getPointCount() const { - return (nr_points); + return (nr_points_); } /** \brief Number of points contained by voxel */ - int nr_points; + int nr_points_; /** \brief 3D voxel centroid */ Eigen::Vector3d mean_; @@ -163,7 +195,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { /** \brief Nd voxel centroid * \note Differs from \ref mean_ when color data is used */ - Eigen::VectorXf centroid; + Eigen::VectorXf centroid_; /** \brief Voxel covariance matrix */ Eigen::Matrix3d cov_; @@ -231,42 +263,15 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { /** \brief Initializes voxel structure. */ - inline void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id) { - LeafDict leaves; - applyFilter(cloud, grid_id, leaves); - - grid_leaves_[grid_id] = leaves; - } - - inline void removeCloud(const std::string &grid_id) { - grid_leaves_.erase(grid_id); - } + void setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id); - inline void createKdtree() { - leaves_.clear(); - for(const auto &kv : grid_leaves_) { - leaves_.insert(kv.second.begin(), kv.second.end()); - } - - leaf_indices_.clear(); - voxel_centroids_ptr_.reset(new PointCloud); - voxel_centroids_ptr_->height = 1; - voxel_centroids_ptr_->is_dense = true; - voxel_centroids_ptr_->points.clear(); - voxel_centroids_ptr_->points.reserve(leaves_.size()); - for(const auto &element : leaves_) { - leaf_indices_.push_back(element.first); - voxel_centroids_ptr_->push_back(PointT()); - voxel_centroids_ptr_->points.back().x = element.second.centroid[0]; - voxel_centroids_ptr_->points.back().y = element.second.centroid[1]; - voxel_centroids_ptr_->points.back().z = element.second.centroid[2]; - } - voxel_centroids_ptr_->width = static_cast(voxel_centroids_ptr_->points.size()); + /** \brief Remove a ND voxel grid corresponding to the specified id + */ + void removeCloud(const std::string &grid_id); - if(voxel_centroids_ptr_->size() > 0) { - kdtree_.setInputCloud(voxel_centroids_ptr_); - } - } + /** \brief Build Kdtrees from the NDT voxel for later radius search + */ + void createKdtree(); /** \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. @@ -277,25 +282,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { * \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) const { - k_leaves.clear(); - - // 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++) { - auto leaf = leaves_.find(leaf_indices_[*iter]); - if(leaf == leaves_.end()) { - std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl; - std::cin.ignore(1); - } - k_leaves.push_back(&(leaf->second)); - } - return k; - } + int radiusSearch(const PointT &point, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; /** \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. @@ -307,22 +294,11 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid { * \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) const { - if(index >= static_cast(cloud.points.size()) || index < 0) return (0); - return (radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); - } + int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; - PointCloud getVoxelPCD() const { - return *voxel_centroids_ptr_; - } + PointCloud getVoxelPCD() const; - std::vector getCurrentMapIDs() const { - std::vector output{}; - for(const auto &element : grid_leaves_) { - output.push_back(element.first); - } - return output; - } + std::vector getCurrentMapIDs() const; protected: /** \brief Filter cloud and initializes voxel structure. diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp index 97cbed57..8d495cb2 100644 --- a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp +++ b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp @@ -56,9 +56,10 @@ #include #include "multigrid_pclomp/multi_voxel_grid_covariance_omp.h" +namespace pclomp { + template -pclomp::MultiVoxelGridCovariance::MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other) - : pcl::VoxelGrid(other), leaves_(other.leaves_), grid_leaves_(other.grid_leaves_), leaf_indices_(other.leaf_indices_), kdtree_(other.kdtree_) { +MultiVoxelGridCovariance::MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other) : pcl::VoxelGrid(other), leaves_(other.leaves_), grid_leaves_(other.grid_leaves_), leaf_indices_(other.leaf_indices_), kdtree_(other.kdtree_) { min_points_per_voxel_ = other.min_points_per_voxel_; min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_; @@ -71,14 +72,14 @@ pclomp::MultiVoxelGridCovariance::MultiVoxelGridCovariance(const MultiVo } template -pclomp::MultiVoxelGridCovariance::MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other) +MultiVoxelGridCovariance::MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other) : pcl::VoxelGrid(std::move(other)), leaves_(std::move(other.leaves_)), grid_leaves_(std::move(other.grid_leaves_)), leaf_indices_(std::move(other.leaf_indices_)), kdtree_(std::move(other.kdtree_)), voxel_centroids_ptr_(std::move(other.voxel_centroids_ptr_)) { min_points_per_voxel_ = other.min_points_per_voxel_; min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_; } template -pclomp::MultiVoxelGridCovariance &pclomp::MultiVoxelGridCovariance::operator=(const MultiVoxelGridCovariance &other) { +MultiVoxelGridCovariance &pclomp::MultiVoxelGridCovariance::operator=(const MultiVoxelGridCovariance &other) { pcl::VoxelGrid::operator=(other); leaves_ = other.leaves_; grid_leaves_ = other.grid_leaves_; @@ -99,7 +100,7 @@ pclomp::MultiVoxelGridCovariance &pclomp::MultiVoxelGridCovariance -pclomp::MultiVoxelGridCovariance &pclomp::MultiVoxelGridCovariance::operator=(MultiVoxelGridCovariance &&other) { +MultiVoxelGridCovariance &pclomp::MultiVoxelGridCovariance::operator=(MultiVoxelGridCovariance &&other) { pcl::VoxelGrid::operator=(std::move(other)); leaves_ = std::move(other.leaves_); grid_leaves_ = std::move(other.grid_leaves_); @@ -112,9 +113,90 @@ pclomp::MultiVoxelGridCovariance &pclomp::MultiVoxelGridCovariance +void MultiVoxelGridCovariance::setInputCloudAndFilter(const PointCloudConstPtr &cloud, const std::string &grid_id) { + LeafDict leaves; + applyFilter(cloud, grid_id, leaves); + + grid_leaves_[grid_id] = leaves; +} + +template +void MultiVoxelGridCovariance::removeCloud(const std::string &grid_id) { + grid_leaves_.erase(grid_id); +} + +template +void MultiVoxelGridCovariance::createKdtree() { + leaves_.clear(); + for(const auto &kv : grid_leaves_) { + leaves_.insert(kv.second.begin(), kv.second.end()); + } + + leaf_indices_.clear(); + voxel_centroids_ptr_.reset(new PointCloud); + voxel_centroids_ptr_->height = 1; + voxel_centroids_ptr_->is_dense = true; + voxel_centroids_ptr_->points.clear(); + voxel_centroids_ptr_->points.reserve(leaves_.size()); + for(const auto &element : leaves_) { + leaf_indices_.push_back(element.first); + voxel_centroids_ptr_->push_back(PointT()); + voxel_centroids_ptr_->points.back().x = element.second.centroid_[0]; + voxel_centroids_ptr_->points.back().y = element.second.centroid_[1]; + voxel_centroids_ptr_->points.back().z = element.second.centroid_[2]; + } + voxel_centroids_ptr_->width = static_cast(voxel_centroids_ptr_->points.size()); + + if(voxel_centroids_ptr_->size() > 0) { + kdtree_.setInputCloud(voxel_centroids_ptr_); + } +} + +template +int MultiVoxelGridCovariance::radiusSearch(const PointT &point, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn) const { + k_leaves.clear(); + + // 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++) { + auto leaf = leaves_.find(leaf_indices_[*iter]); + if(leaf == leaves_.end()) { + std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl; + std::cin.ignore(1); + } + k_leaves.push_back(&(leaf->second)); + } + return k; +} + +template +int MultiVoxelGridCovariance::radiusSearch(const PointCloud &cloud, int index, double radius, std::vector &k_leaves, std::vector &k_sqr_distances, unsigned int max_nn) const { + if(index >= static_cast(cloud.points.size()) || index < 0) return (0); + return (radiusSearch(cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); +} + +template +typename MultiVoxelGridCovariance::PointCloud MultiVoxelGridCovariance::getVoxelPCD() const { + return *voxel_centroids_ptr_; +} + +template +std::vector MultiVoxelGridCovariance::getCurrentMapIDs() const { + std::vector output{}; + for(const auto &element : grid_leaves_) { + output.push_back(element.first); + } + return output; +} + ////////////////////////////////////////////////////////////////////////////////////////// template -void pclomp::MultiVoxelGridCovariance::applyFilter(const PointCloudConstPtr &input, const std::string &grid_id, LeafDict &leaves) const { +void MultiVoxelGridCovariance::applyFilter(const PointCloudConstPtr &input, const std::string &grid_id, LeafDict &leaves) const { // Has the input dataset been set already? if(!input) { PCL_WARN("[pcl::%s::applyFilter] No input dataset given!\n", getClassName().c_str()); @@ -180,15 +262,15 @@ void pclomp::MultiVoxelGridCovariance::applyFilter(const PointCloudConst Leaf &leaf = it->second; // Normalize the centroid - leaf.centroid /= static_cast(leaf.nr_points); + 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; + leaf.mean_ /= leaf.nr_points_; // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and voxel_grid_info.voxel_centroids 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(leaf.nr_points_ >= min_points_per_voxel_) { computeLeafParams(pt_sum, eigensolver, leaf); } else { leaf_ids_to_remove.push_back(it->first); @@ -202,15 +284,15 @@ void pclomp::MultiVoxelGridCovariance::applyFilter(const PointCloudConst } template -void pclomp::MultiVoxelGridCovariance::updateVoxelCentroids(const Leaf &leaf, PointCloud &voxel_centroids) const { +void MultiVoxelGridCovariance::updateVoxelCentroids(const Leaf &leaf, PointCloud &voxel_centroids) const { voxel_centroids.push_back(PointT()); - voxel_centroids.points.back().x = leaf.centroid[0]; - voxel_centroids.points.back().y = leaf.centroid[1]; - voxel_centroids.points.back().z = leaf.centroid[2]; + voxel_centroids.points.back().x = leaf.centroid_[0]; + voxel_centroids.points.back().y = leaf.centroid_[1]; + voxel_centroids.points.back().z = leaf.centroid_[2]; } template -typename pclomp::MultiVoxelGridCovariance::LeafID pclomp::MultiVoxelGridCovariance::getLeafID(const std::string &grid_id, const PointT &point, const BoundingBox &bbox) const { +typename MultiVoxelGridCovariance::LeafID pclomp::MultiVoxelGridCovariance::getLeafID(const std::string &grid_id, const PointT &point, const BoundingBox &bbox) const { int ijk0 = static_cast(floor(point.x * inverse_leaf_size_[0]) - static_cast(bbox.min[0])); int ijk1 = static_cast(floor(point.y * inverse_leaf_size_[1]) - static_cast(bbox.min[1])); int ijk2 = static_cast(floor(point.z * inverse_leaf_size_[2]) - static_cast(bbox.min[2])); @@ -223,9 +305,9 @@ typename pclomp::MultiVoxelGridCovariance::LeafID pclomp::MultiVoxelGrid template void pclomp::MultiVoxelGridCovariance::updateLeaf(const PointT &point, const int ¢roid_size, Leaf &leaf) const { - if(leaf.nr_points == 0) { - leaf.centroid.resize(centroid_size); - leaf.centroid.setZero(); + if(leaf.nr_points_ == 0) { + leaf.centroid_.resize(centroid_size); + leaf.centroid_.setZero(); } Eigen::Vector3d pt3d(point.x, point.y, point.z); @@ -235,15 +317,15 @@ void pclomp::MultiVoxelGridCovariance::updateLeaf(const PointT &point, c leaf.cov_ += pt3d * pt3d.transpose(); Eigen::Vector4f pt(point.x, point.y, point.z, 0); - leaf.centroid.template head<4>() += pt; - ++leaf.nr_points; + leaf.centroid_.template head<4>() += pt; + ++leaf.nr_points_; } template void pclomp::MultiVoxelGridCovariance::computeLeafParams(const Eigen::Vector3d &pt_sum, Eigen::SelfAdjointEigenSolver &eigensolver, Leaf &leaf) const { // Single pass covariance calculation - leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose(); - leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; + leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose())) / leaf.nr_points_ + leaf.mean_ * leaf.mean_.transpose(); + leaf.cov_ *= (leaf.nr_points_ - 1.0) / leaf.nr_points_; // Normalize Eigen Val such that max no more than 100x min. eigensolver.compute(leaf.cov_); @@ -251,7 +333,7 @@ void pclomp::MultiVoxelGridCovariance::computeLeafParams(const Eigen::Ve leaf.evecs_ = eigensolver.eigenvectors(); if(eigen_val(0, 0) < 0 || eigen_val(1, 1) < 0 || eigen_val(2, 2) <= 0) { - leaf.nr_points = -1; + leaf.nr_points_ = -1; return; } @@ -270,10 +352,12 @@ void pclomp::MultiVoxelGridCovariance::computeLeafParams(const Eigen::Ve leaf.icov_ = leaf.cov_.inverse(); if(leaf.icov_.maxCoeff() == std::numeric_limits::infinity() || leaf.icov_.minCoeff() == -std::numeric_limits::infinity()) { - leaf.nr_points = -1; + leaf.nr_points_ = -1; } } +} // namespace pclomp + #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance; #endif // PCL_MULTI_VOXEL_GRID_COVARIANCE_IMPL_H_ diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h index a609c781..d4aa7e87 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp.h +++ b/include/multigrid_pclomp/multigrid_ndt_omp.h @@ -179,7 +179,7 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration::setInputTarget(cloud); target_cells_.setLeafSize(resolution_, resolution_, resolution_); target_cells_.setInputCloudAndFilter(cloud, target_id); diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index fce03c09..935ba3b9 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -1090,7 +1090,7 @@ double pclomp::MultiGridNormalDistributionsTransform:: template double pclomp::MultiGridNormalDistributionsTransform::calculateNearestVoxelTransformationLikelihood(const PointCloudSource &trans_cloud) const { double nearest_voxel_score = 0; - size_t found_neigborhood_voxel_num = 0; + size_t found_neighborhood_voxel_num = 0; for(std::size_t idx = 0; idx < trans_cloud.points.size(); idx++) { double nearest_voxel_score_pt = 0; @@ -1124,14 +1124,14 @@ double pclomp::MultiGridNormalDistributionsTransform:: } if(!neighborhood.empty()) { - ++found_neigborhood_voxel_num; + ++found_neighborhood_voxel_num; nearest_voxel_score += nearest_voxel_score_pt; } } double output_score = 0; - if(found_neigborhood_voxel_num != 0) { - output_score = nearest_voxel_score / static_cast(found_neigborhood_voxel_num); + if(found_neighborhood_voxel_num != 0) { + output_score = nearest_voxel_score / static_cast(found_neighborhood_voxel_num); } return output_score; }