diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index ab41c347..46dd564e 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -195,7 +195,6 @@ void pclomp::MultiGridNormalDistributionsTransform::co } Eigen::Transform eig_transformation; - eig_transformation.matrix() = final_transformation_; transformation_array_.clear(); transformation_array_.push_back(final_transformation_); @@ -204,7 +203,6 @@ void pclomp::MultiGridNormalDistributionsTransform::co Eigen::Matrix p, delta_p, score_gradient; Eigen::Vector3f init_translation = eig_transformation.translation(); Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); - p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), init_rotation(1), init_rotation(2); Eigen::Matrix hessian; @@ -290,7 +288,6 @@ template double pclomp::MultiGridNormalDistributionsTransform::computeDerivatives(Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, PointCloudSource &trans_cloud, Eigen::Matrix &p, bool compute_hessian) { score_gradient.setZero(); hessian.setZero(); - double score = 0; int total_neighborhood_count = 0; double nearest_voxel_score = 0; @@ -303,7 +300,6 @@ double pclomp::MultiGridNormalDistributionsTransform:: std::vector, Eigen::aligned_allocator>> score_gradients(input_size); std::vector, Eigen::aligned_allocator>> hessians(input_size); std::vector neighborhood_counts(input_size); - for(size_t i = 0; i < input_size; ++i) { scores[i] = 0; nearest_voxel_scores[i] = 0;