Fixed signature

Signed-off-by: Shintaro Sakoda <[email protected]>

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);
 

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<double, 6, 6>& hessian);
 Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& offset_y);
 Eigen::Matrix2d estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& 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<Eigen::Matrix4f> propose_search_points(const Eigen::Matrix<double, 6, 6>& hessian, const Eigen::Matrix4f& center_pose, const std::vector<double>& offset_x, const std::vector<double>& offset_y);
+
 /** \brief Calculate weights by exponential */
 std::vector<double> calc_weight_vec(const std::vector<double>& score_vec, double temperature);
 

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

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<double, 6, 6>& hessian = ndt_result.hessian;
+Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const Eigen::Matrix<double, 6, 6>& 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<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& 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<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& 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<Eigen::Matrix4f> propose_search_points(const Eigen::Matrix<double, 6, 6>& hessian, const Eigen::Matrix4f& center_pose, const std::vector<double>& offset_x, const std::vector<double>& 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<Eigen::Matrix4f> search_points;
+  for(int i = 0; i < static_cast<int>(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<float>(rotated_pose_offset_2d.x());
+    search_point(1, 3) += static_cast<float>(rotated_pose_offset_2d.y());
+    search_points.emplace_back(search_point);
+  }
+  return search_points;
+}
+
 std::vector<double> calc_weight_vec(const std::vector<double>& score_vec, double temperature) {
   const int n = static_cast<int>(score_vec.size());
   const double max_score = *std::max_element(score_vec.begin(), score_vec.end());