diff --git a/src/estimate_covariance/estimate_covariance.cpp b/src/estimate_covariance/estimate_covariance.cpp index 4e71bb0a..8063530e 100644 --- a/src/estimate_covariance/estimate_covariance.cpp +++ b/src/estimate_covariance/estimate_covariance.cpp @@ -91,9 +91,14 @@ std::vector propose_poses_to_search(const NdtResult& ndt_result assert(offset_x.size() == offset_y.size()); const Eigen::Matrix& hessian = ndt_result.hessian; const Eigen::Matrix4f& center_pose = ndt_result.pose; - const Eigen::Matrix2d covariance = estimate_xy_covariance_by_Laplace_approximation(hessian); - // const Eigen::Matrix2d rot = find_rotation_matrix_aligning_covariance_to_principal_axes(-covariance); + + // (1) calculate rot by pose (default) const Eigen::Matrix2d rot = ndt_result.pose.topLeftCorner<2, 2>().cast(); + + // (2) calculate rot by covariance (alternative) + // const Eigen::Matrix2d covariance = estimate_xy_covariance_by_Laplace_approximation(hessian); + // const Eigen::Matrix2d rot = find_rotation_matrix_aligning_covariance_to_principal_axes(-covariance); + std::vector poses_to_search; for(int i = 0; i < static_cast(offset_x.size()); i++) { const Eigen::Vector2d pose_offset(offset_x[i], offset_y[i]);