add covariance_estimation (#22)

tier4 · Oct 27, 2023 · 03f63ed · 03f63ed
1 parent 772e316
commit 03f63ed
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 0 deletions.
diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h
@@ -71,6 +71,7 @@ namespace pclomp
 		float transform_probability;
 		float nearest_voxel_transformation_likelihood;
 		int iteration_num;
+		Eigen::Matrix<double, 6, 6> hessian;
 		std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
 		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 	};
@@ -336,6 +337,7 @@ namespace pclomp
 			ndt_result.nearest_voxel_transformation_likelihood =
 				getNearestVoxelTransformationLikelihood();
 			ndt_result.iteration_num = getFinalNumIteration();
+			ndt_result.hessian = getHessian();
 			return ndt_result;
 		}
 

diff --git a/include/pclomp/ndt_omp.h b/include/pclomp/ndt_omp.h
@@ -65,6 +65,7 @@ namespace pclomp
 		float nearest_voxel_transformation_likelihood;
 		int iteration_num;
 		std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
+		Eigen::Matrix<double, 6, 6> hessian;
 		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 	};
 
@@ -318,6 +319,7 @@ namespace pclomp
 			ndt_result.nearest_voxel_transformation_likelihood =
 				getNearestVoxelTransformationLikelihood();
 			ndt_result.iteration_num = getFinalNumIteration();
+			ndt_result.hessian = getHessian();
 			return ndt_result;
 		}