diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp index 7ac99c6c..1c254f3e 100644 --- a/apps/check_covariance.cpp +++ b/apps/check_covariance.cpp @@ -133,7 +133,7 @@ int main(int argc, char** argv) { std::cout << source_pcd << ", num=" << std::setw(4) << source_cloud->size() << " points, time=" << elapsed << " [msec], score=" << score << std::endl; // estimate covariance - const Eigen::Matrix2d cov_by_la = pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result); + const Eigen::Matrix2d cov_by_la = pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); const Eigen::Matrix2d cov_by_mndt = pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y); const Eigen::Matrix2d cov_by_mndt_score = pclomp::estimate_xy_covariance_by_multi_ndt_score(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y); diff --git a/include/estimate_covariance/estimate_covariance.hpp b/include/estimate_covariance/estimate_covariance.hpp index 0cdcf473..8015b1b0 100644 --- a/include/estimate_covariance/estimate_covariance.hpp +++ b/include/estimate_covariance/estimate_covariance.hpp @@ -7,10 +7,17 @@ namespace pclomp { Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(const Eigen::Matrix2d& matrix); -Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const NdtResult& ndt_result); +Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const Eigen::Matrix& hessian); Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y); Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y); +/** \brief Propose search points. + * (1) Compute covariance by Laplace approximation + * (2) Find rotation matrix aligning covariance to principal axes + * (3) Propose search points by adding offset_x and offset_y to the center_pose + */ +std::vector propose_search_points(const Eigen::Matrix& hessian, const Eigen::Matrix4f& center_pose, const std::vector& offset_x, const std::vector& offset_y); + /** \brief Calculate weights by exponential */ std::vector calc_weight_vec(const std::vector& score_vec, double temperature); diff --git a/script/execute_check_covariance.sh b/script/execute_check_covariance.sh index 0e754a1e..9be07ff5 100755 --- a/script/execute_check_covariance.sh +++ b/script/execute_check_covariance.sh @@ -4,5 +4,8 @@ set -eux cd $(dirname $0)/../build make -j8 -./check_covariance ../check_covariance_data/input/ ../check_covariance_data/output -python3 ../script/plot_covariance.py ../check_covariance_data/output/result.csv +./check_covariance ../check_covariance_data/input_awsim_nishishinjuku/ ../check_covariance_data/output_awsim_nishishinjuku +python3 ../script/plot_covariance.py ../check_covariance_data/output_awsim_nishishinjuku/result.csv + +./check_covariance ../check_covariance_data/input_tunnel ../check_covariance_data/output_tunnel +python3 ../script/plot_covariance.py ../check_covariance_data/output_tunnel/result.csv diff --git a/src/estimate_covariance/estimate_covariance.cpp b/src/estimate_covariance/estimate_covariance.cpp index a1a8500d..12d94d04 100644 --- a/src/estimate_covariance/estimate_covariance.cpp +++ b/src/estimate_covariance/estimate_covariance.cpp @@ -14,15 +14,14 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(const throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const NdtResult& ndt_result) { - const Eigen::Matrix& hessian = ndt_result.hessian; +Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const Eigen::Matrix& hessian) { const Eigen::Matrix2d hessian_xy = hessian.block<2, 2>(0, 0); const Eigen::Matrix2d covariance_xy = -hessian_xy.inverse(); return covariance_xy; } Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y) { - const Eigen::Matrix2d cov_by_la = estimate_xy_covariance_by_Laplace_approximation(ndt_result); + const Eigen::Matrix2d cov_by_la = estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); const Eigen::Matrix2d rot = find_rotation_matrix_aligning_covariance_to_principal_axes(-cov_by_la); assert(offset_x.size() == offset_y.size()); @@ -72,7 +71,7 @@ Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, } Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y) { - const Eigen::Matrix2d cov_by_la = estimate_xy_covariance_by_Laplace_approximation(ndt_result); + const Eigen::Matrix2d cov_by_la = estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); const Eigen::Matrix2d rot = find_rotation_matrix_aligning_covariance_to_principal_axes(-cov_by_la); assert(offset_x.size() == offset_y.size()); @@ -125,6 +124,22 @@ Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_r return covariance; } +std::vector propose_search_points(const Eigen::Matrix& hessian, const Eigen::Matrix4f& center_pose, const std::vector& offset_x, const std::vector& offset_y) { + assert(offset_x.size() == offset_y.size()); + 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 search_points; + for(int i = 0; i < static_cast(offset_x.size()); i++) { + const Eigen::Vector2d pose_offset(offset_x[i], offset_y[i]); + const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; + Eigen::Matrix4f search_point = center_pose; + search_point(0, 3) += static_cast(rotated_pose_offset_2d.x()); + search_point(1, 3) += static_cast(rotated_pose_offset_2d.y()); + search_points.emplace_back(search_point); + } + return search_points; +} + std::vector calc_weight_vec(const std::vector& score_vec, double temperature) { const int n = static_cast(score_vec.size()); const double max_score = *std::max_element(score_vec.begin(), score_vec.end());