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_);