diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h index 5c92480..abd366a 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp.h +++ b/include/multigrid_pclomp/multigrid_ndt_omp.h @@ -264,8 +264,6 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration::Ptr calculateNearestVoxelScoreEachPoint(const PointCloudSource & cloud) const; - //inline pcl::PointCloud::Ptr getScorePoints() const { return (score_points_); } - inline void setRegularizationScaleFactor(float regularization_scale_factor) { params_.regularization_scale_factor = regularization_scale_factor; diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index 87af61c..cb03174 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -1157,6 +1157,7 @@ double MultiGridNormalDistributionsTransform:: nearest_voxel_score_pt = score_inc; } } + t_nvs[tid] += nearest_voxel_score_pt; ++t_found_nnvn[tid]; }