From 113dae1a36c8da20f77e852ddb81090537c6a285 Mon Sep 17 00:00:00 2001 From: lizhengsi Date: Sun, 8 Jun 2025 09:01:37 +0000 Subject: [PATCH] fix voxel covariance --- include/pclomp/voxel_grid_covariance_omp.h | 2 +- include/pclomp/voxel_grid_covariance_omp_impl.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pclomp/voxel_grid_covariance_omp.h b/include/pclomp/voxel_grid_covariance_omp.h index 725e8fd3..98da6b6c 100644 --- a/include/pclomp/voxel_grid_covariance_omp.h +++ b/include/pclomp/voxel_grid_covariance_omp.h @@ -104,7 +104,7 @@ namespace pclomp nr_points (0), mean_ (Eigen::Vector3d::Zero ()), centroid (), - cov_ (Eigen::Matrix3d::Identity ()), + cov_ (Eigen::Matrix3d::Zero ()), icov_ (Eigen::Matrix3d::Zero ()), evecs_ (Eigen::Matrix3d::Identity ()), evals_ (Eigen::Vector3d::Zero ()) diff --git a/include/pclomp/voxel_grid_covariance_omp_impl.hpp b/include/pclomp/voxel_grid_covariance_omp_impl.hpp index 5b579a39..31e1678e 100644 --- a/include/pclomp/voxel_grid_covariance_omp_impl.hpp +++ b/include/pclomp/voxel_grid_covariance_omp_impl.hpp @@ -327,7 +327,7 @@ pclomp::VoxelGridCovariance::applyFilter (PointCloud &output) // 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.nr_points / (leaf.nr_points - 1.0); //Normalize Eigen Val such that max no more than 100x min. eigensolver.compute (leaf.cov_);