From 106cbaa79e6ab20da1e8c28c527f9a0d427f1e40 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Fri, 22 Nov 2024 20:58:14 +0900 Subject: [PATCH 1/2] perf: remove evecs_, evals_ --- include/pclomp/voxel_grid_covariance_omp.h | 31 +------------------ .../pclomp/voxel_grid_covariance_omp_impl.hpp | 6 ++-- 2 files changed, 4 insertions(+), 33 deletions(-) diff --git a/include/pclomp/voxel_grid_covariance_omp.h b/include/pclomp/voxel_grid_covariance_omp.h index 725e8fd3..340b82c2 100644 --- a/include/pclomp/voxel_grid_covariance_omp.h +++ b/include/pclomp/voxel_grid_covariance_omp.h @@ -98,7 +98,7 @@ namespace pclomp 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 cov_ to the identity matrix */ Leaf () : nr_points (0), @@ -106,8 +106,6 @@ namespace pclomp centroid (), cov_ (Eigen::Matrix3d::Identity ()), icov_ (Eigen::Matrix3d::Zero ()), - evecs_ (Eigen::Matrix3d::Identity ()), - evals_ (Eigen::Vector3d::Zero ()) { } @@ -138,26 +136,6 @@ namespace pclomp return (mean_); } - /** \brief Get the eigen vectors of the voxel covariance. - * \note Order corresponds with \ref getEvals - * \return matrix whose columns contain eigen vectors - */ - Eigen::Matrix3d - getEvecs () const - { - return (evecs_); - } - - /** \brief Get the eigen values of the voxel covariance. - * \note Order corresponds with \ref getEvecs - * \return vector of eigen values - */ - Eigen::Vector3d - getEvals () const - { - return (evals_); - } - /** \brief Get the number of points contained by this voxel. * \return number of points */ @@ -183,13 +161,6 @@ namespace pclomp /** \brief Inverse of voxel covariance matrix */ Eigen::Matrix3d icov_; - - /** \brief Eigen vectors of voxel covariance matrix */ - Eigen::Matrix3d evecs_; - - /** \brief Eigen values of voxel covariance matrix */ - Eigen::Vector3d evals_; - }; /** \brief Pointer to VoxelGridCovariance leaf structure */ diff --git a/include/pclomp/voxel_grid_covariance_omp_impl.hpp b/include/pclomp/voxel_grid_covariance_omp_impl.hpp index 5b579a39..4b6896ae 100644 --- a/include/pclomp/voxel_grid_covariance_omp_impl.hpp +++ b/include/pclomp/voxel_grid_covariance_omp_impl.hpp @@ -274,6 +274,7 @@ pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) // Eigen values and vectors calculated to prevent near singular matrices Eigen::SelfAdjointEigenSolver eigensolver; Eigen::Matrix3d eigen_val; + Eigen::Matrix3d eigen_vec; 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. @@ -332,7 +333,7 @@ pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) //Normalize Eigen Val such that max no more than 100x min. eigensolver.compute (leaf.cov_); eigen_val = eigensolver.eigenvalues ().asDiagonal (); - leaf.evecs_ = eigensolver.eigenvectors (); + eigen_vec = eigensolver.eigenvectors (); if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0) { @@ -352,9 +353,8 @@ pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) eigen_val (1, 1) = min_covar_eigvalue; } - leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse (); + leaf.cov_ = eigen_vec * eigen_val * eigen_vec.inverse (); } - leaf.evals_ = eigen_val.diagonal (); leaf.icov_ = leaf.cov_.inverse (); if (leaf.icov_.maxCoeff () == std::numeric_limits::infinity ( ) From 6033ae12f6b51d08bdb0c2bbf3d80d33c55fa655 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Mon, 2 Dec 2024 17:26:13 +0900 Subject: [PATCH 2/2] fix --- include/pclomp/voxel_grid_covariance_omp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pclomp/voxel_grid_covariance_omp.h b/include/pclomp/voxel_grid_covariance_omp.h index 340b82c2..2029d317 100644 --- a/include/pclomp/voxel_grid_covariance_omp.h +++ b/include/pclomp/voxel_grid_covariance_omp.h @@ -105,7 +105,7 @@ namespace pclomp mean_ (Eigen::Vector3d::Zero ()), centroid (), cov_ (Eigen::Matrix3d::Identity ()), - icov_ (Eigen::Matrix3d::Zero ()), + icov_ (Eigen::Matrix3d::Zero ()) { }