From a3b27234eb56c9dd9f69db756caf67c760a5ce40 Mon Sep 17 00:00:00 2001 From: Kento Osa <38522559+taisa1@users.noreply.github.com> Date: Wed, 13 Nov 2024 04:07:43 +0900 Subject: [PATCH] perf(autoware_ndt_scan_matcher): remove evecs_, evals_ of Leaf for memory efficiency (#9281) * fix(lane_change): correct computation of maximum lane changing length threshold (#9279) fix computation of maximum lane changing length threshold Signed-off-by: mohammad alqudah Signed-off-by: taisa1 * perf: remove evecs, evals from Leaf Signed-off-by: taisa1 * perf: remove evecs, evals from Leaf Signed-off-by: taisa1 --------- Signed-off-by: mohammad alqudah Signed-off-by: taisa1 Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../ndt_omp/multi_voxel_grid_covariance_omp.h | 41 ++----------------- .../multi_voxel_grid_covariance_omp_impl.hpp | 5 +-- 2 files changed, 5 insertions(+), 41 deletions(-) diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h index 85f37e878bd48..0e43b0b35aecd 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h @@ -130,19 +130,12 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid mean_(Eigen::Vector3d::Zero()), centroid_(), cov_(Eigen::Matrix3d::Identity()), - icov_(Eigen::Matrix3d::Zero()), - evecs_(Eigen::Matrix3d::Identity()), - evals_(Eigen::Vector3d::Zero()) + icov_(Eigen::Matrix3d::Zero()) { } Leaf(const Leaf & other) - : mean_(other.mean_), - centroid_(other.centroid_), - cov_(other.cov_), - icov_(other.icov_), - evecs_(other.evecs_), - evals_(other.evals_) + : mean_(other.mean_), centroid_(other.centroid_), cov_(other.cov_), icov_(other.icov_) { nr_points_ = other.nr_points_; } @@ -151,9 +144,7 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid : 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_)) + icov_(std::move(other.icov_)) { nr_points_ = other.nr_points_; } @@ -164,8 +155,6 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid centroid_ = other.centroid_; cov_ = other.cov_; icov_ = other.icov_; - evecs_ = other.evecs_; - evals_ = other.evals_; nr_points_ = other.nr_points_; return *this; @@ -177,8 +166,6 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid 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; @@ -204,22 +191,6 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid Eigen::Vector3d & getMean() { return (mean_); } - /** \brief Get the eigen vectors of the voxel covariance. - * \note Order corresponds with \ref getEvals - * \return matrix whose columns contain eigen vectors - */ - const Eigen::Matrix3d & getEvecs() const { return (evecs_); } - - Eigen::Matrix3d & getEvecs() { return (evecs_); } - - /** \brief Get the eigen values of the voxel covariance. - * \note Order corresponds with \ref getEvecs - * \return vector of eigen values - */ - const Eigen::Vector3d & getEvals() const { return (evals_); } - - Eigen::Vector3d & getEvals() { return (evals_); } - /** \brief Get the number of points contained by this voxel. * \return number of points */ @@ -241,12 +212,6 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid /** \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 MultiVoxelGridCovariance leaf structure */ diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp index ef64471dd4784..3d7076bbae06d 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp @@ -441,7 +441,7 @@ void pclomp::MultiVoxelGridCovariance::computeLeafParams( // Normalize Eigen Val such that max no more than 100x min. eigensolver.compute(leaf.cov_); Eigen::Matrix3d eigen_val = eigensolver.eigenvalues().asDiagonal(); - leaf.evecs_ = eigensolver.eigenvectors(); + Eigen::Matrix3d eigen_vec = eigensolver.eigenvectors(); if (eigen_val(0, 0) < 0 || eigen_val(1, 1) < 0 || eigen_val(2, 2) <= 0) { leaf.nr_points_ = -1; @@ -457,9 +457,8 @@ void pclomp::MultiVoxelGridCovariance::computeLeafParams( 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 (