Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

perf(autoware_ndt_scan_matcher): remove evecs_, evals_ of Leaf for memory efficiency #9281

Merged
merged 4 commits into from
Nov 12, 2024
Merged

perf(autoware_ndt_scan_matcher): remove evecs_, evals_ of Leaf for memory efficiency #9281

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
dd8a36d
fix(lane_change): correct computation of maximum lane changing length…
mkquda Nov 11, 2024
158928f
perf: remove evecs, evals from Leaf
taisa1 Nov 11, 2024
d98d397
perf: remove evecs, evals from Leaf
taisa1 Nov 11, 2024
694d80e
Merge branch 'perf_ndt_scan_matcher_shrink_leaf' of github.com:TAISA1…
taisa1 Nov 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 3 additions & 38 deletions ..._scan_matcher/include/autoware/ndt_scan_matcher/ndt_omp/multi_voxel_grid_covariance_omp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
}
Expand All @@ -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_;
}
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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
*/
Expand All @@ -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 */
Expand Down
5 changes: 2 additions & 3 deletions localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,7 @@
// 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;
Expand All @@ -457,9 +457,8 @@
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();

Check warning on line 460 in localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp

View check run for this annotation

Codecov / codecov/patch

localization/autoware_ndt_scan_matcher/src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp#L460 

Added line #L460 was not covered by tests
}
leaf.evals_ = eigen_val.diagonal();

leaf.icov_ = leaf.cov_.inverse();
if (
Expand Down
Loading