perf(autoware_ndt_scan_matcher): remove evecs_, evals_ of Leaf for me…

…mory efficiency (autowarefoundation#9281) * fix(lane_change): correct computation of maximum lane changing length threshold (autowarefoundation#9279) fix computation of maximum lane changing length threshold Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> Signed-off-by: taisa1 <kento.osa@tier4.jp> * perf: remove evecs, evals from Leaf Signed-off-by: taisa1 <kento.osa@tier4.jp> * perf: remove evecs, evals from Leaf Signed-off-by: taisa1 <kento.osa@tier4.jp> --------- Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> Signed-off-by: taisa1 <kento.osa@tier4.jp> Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>
1222-takeshi · Nov 12, 2024 · a3b2723 · a3b2723
1 parent 3a92654
commit a3b2723
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 41 deletions.
diff --git a/..._scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h b/..._scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h
@@ -130,19 +130,12 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT>
       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<PointT>
     : 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<PointT>
       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<PointT>
       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<PointT>
 
     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<PointT>
 
     /** \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
@@ -441,7 +441,7 @@ void pclomp::MultiVoxelGridCovariance<PointT>::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<PointT>::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 (