From 30463a3c5d8460370267b674dddf15327e471919 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Tue, 11 Mar 2025 00:30:54 +0900 Subject: [PATCH] Fix the covariance calculation Signed-off-by: Anh Nguyen --- .../src/ndt_omp/multi_voxel_grid_covariance_omp_impl.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 3d7076bbae06d..1416729087966 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 @@ -434,9 +434,7 @@ void pclomp::MultiVoxelGridCovariance::computeLeafParams( 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_ - pt_sum * leaf.mean_.transpose()) / (leaf.nr_points_ - 1); // Normalize Eigen Val such that max no more than 100x min. eigensolver.compute(leaf.cov_);