Ros2 v0.8.0 ndt scan matcher (autowarefoundation#266)

* restore file name for v0.8.0 update Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * delete std::cout (autowarefoundation#998) Signed-off-by: Yamato Ando <yamato.ando@gmail.com> * Add ndt_scan_matcher.yaml (autowarefoundation#1122) Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp> * Revert "restore file name for v0.8.0 update" This reverts commit 38869185b4f423a2656f18f71a18de30d1a83730. Co-authored-by: YamatoAndo <yamato.ando@gmail.com> Co-authored-by: Yuma Nihei <yuma.nihei@tier4.jp>
kosuke55 · Jan 30, 2021 · b134f6e · b134f6e
1 parent 4cf182c
commit b134f6e
Show file tree

Hide file tree

Showing 6 changed files with 43 additions and 54 deletions.
diff --git a/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp b/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp
@@ -166,14 +166,8 @@ void ndt_omp::NormalDistributionsTransform<PointSource, PointTarget>::computeTra
     // Eigen::Matrix<double, 6, 1> score_gradient_rotation = score_gradient;
     // score_gradient_rotation(0) = score_gradient_rotation(1) = score_gradient_rotation(2) = 0;
 
-    // std::cout << "delta_p: "<< delta_p << std::endl;
-    // std::cout << "score_gradient: "<< score_gradient << std::endl;
     delta_p.normalize();
     // delta_p_rotation.normalize();
-    // std::cout << "score: " << score << std::endl;
-    // std::cout << "delta_p_norm: " << delta_p_norm << std::endl;
-    // std::cout << "delta_p_rotation: " << delta_p_rotation_norm << std::endl;
-    // std::cout << "converged_rotation: " << converged_rotation << std::endl;
 
     // if(!converged_rotation && delta_p_rotation_norm > 0.001 && nr_iterations_ < 10)
     // {

diff --git a/...on/pose_estimator/ndt_scan_matcher/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp b/...on/pose_estimator/ndt_scan_matcher/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp
@@ -130,14 +130,8 @@ void pcl::NormalDistributionsTransformModified<PointSource, PointTarget>::comput
     Eigen::Matrix<double, 6, 1> score_gradient_rotation = score_gradient;
     score_gradient_rotation(0) = score_gradient_rotation(1) = score_gradient_rotation(2) = 0;
 
-    // std::cout << "delta_p: "<< delta_p << std::endl;
-    // std::cout << "score_gradient: "<< score_gradient << std::endl;
     delta_p.normalize();
     delta_p_rotation.normalize();
-    std::cout << "score: " << score << std::endl;
-    std::cout << "delta_p_norm: " << delta_p_norm << std::endl;
-    std::cout << "delta_p_rotation: " << delta_p_rotation_norm << std::endl;
-    // std::cout << "converged_rotation: " << converged_rotation << std::endl;
 
     if (!converged_rotation && delta_p_rotation_norm > 0.001 && nr_iterations_ < 10) {
       delta_p = delta_p_rotation;

diff --git a/localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/CMakeLists.txt b/localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/CMakeLists.txt
@@ -24,4 +24,8 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_auto_package(INSTALL_TO_SHARE launch)
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)
diff --git a/localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/config/ndt_scan_matcher.yaml b/localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/config/ndt_scan_matcher.yaml
@@ -0,0 +1,35 @@
+/**:
+  ros__parameters:
+
+    # Vehicle reference frame
+    base_frame: "base_link"
+
+    # Subscriber queue size
+    input_sensor_points_queue_size: 1
+
+    # NDT implementation type
+    # 0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP
+    ndt_implement_type: 2
+
+    # The maximum difference between two consecutive
+    # transformations in order to consider convergence
+    trans_epsilon: 0.01
+
+    # The newton line search maximum step length
+    step_size: 0.1
+
+    # The ND voxel grid resolution
+    resolution: 2.0
+
+    # The number of iterations required to calculate alignment
+    max_iterations: 30
+
+    # Threshold for deciding whetherto trust the estimation result
+    converged_param_transform_probability: 3.0
+
+    # neighborhood search method in OMP
+    # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
+    omp_neighborhood_search_method: 0
+
+    # Number of threads used for parallel computing
+    omp_num_threads: 4
diff --git a/...ation/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/...ation/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
@@ -1,40 +1,21 @@
 <launch>
+  <arg name="param_file" default="$(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.yaml" />
 
   <!-- Topics -->
   <arg name="input_sensor_points_topic" default="/points_raw" doc="Sensor points topic" />
-
-  <arg name="input_sensor_points_queue_size" default="1" doc="Subscriber queue size" />
   <arg name="input_initial_pose_topic" default="/ekf_pose_with_covariance" doc="Initial position topic to align" />
   <arg name="input_map_points_topic" default="/pointcloud_map" doc="Map points topic" />
 
   <arg name="output_pose_topic" default="ndt_pose" doc="Estimated self position" />
   <arg name="output_pose_with_covariance_topic" default="ndt_pose_with_covariance" doc="Estimated self position with covariance" />
   <arg name="output_diagnostics_topic" default="/diagnostics" doc="Diagnostic topic" />
 
-  <!-- tf -->
-  <arg name="base_frame" default="base_link" doc="Vehicle reference frame" />
-
-  <!-- NDT Params -->
-  <arg name="ndt_implement_type" default="2" doc="0: PCL_GENERIC, 1: PCL_MODIFIED, 2: OMP" />
-
-  <arg name="trans_epsilon" default="0.01" doc="The maximum difference between two consecutive transformations in order to consider convergence" />
-  <arg name="step_size" default="0.1" doc="The newton line search maximum step length" />
-  <arg name="resolution" default="2.0" doc="The ND voxel grid resolution" />
-  <arg name="max_iterations" default="30" doc="The number of iterations required to calculate alignment" />
-
-  <arg name="converged_param_transform_probability" default="3.0" doc="" />
-
-  <!-- OMP Params -->
-  <arg name="omp_neighborhood_search_method" default="0" doc="0:KDTREE, 1:DIRECT26, 2:DIRECT7, 3:DIRECT1" />
-  <arg name="omp_num_threads" default="4" doc="" />
-
 
   <arg name="node_name" default="ndt_scan_matcher" doc="Use a different name for this node" />
 
   <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="log">
     <param name="use_sim_time" value="$(env AW_ROS2_USE_SIM_TIME false)" />
     <remap from="points_raw" to="$(var input_sensor_points_topic)" />
-    <param name="input_sensor_points_queue_size" value="$(var input_sensor_points_queue_size)" />
 
     <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)" />
     <remap from="pointcloud_map" to="$(var input_map_points_topic)" />
@@ -43,19 +24,7 @@
     <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)" />
     <remap from="diagnostics" to="$(var output_diagnostics_topic)" />
 
-    <param name="base_frame" value="$(var base_frame)" />
-
-    <param name="ndt_implement_type" value="$(var ndt_implement_type)" />
-
-    <param name="trans_epsilon" value="$(var trans_epsilon)" />
-    <param name="step_size" value="$(var step_size)" />
-    <param name="resolution" value="$(var resolution)" />
-    <param name="max_iterations" value="$(var max_iterations)" />
-
-    <param name="converged_param_transform_probability" value="$(var converged_param_transform_probability)" />
-
-    <param name="omp_neighborhood_search_method" value="$(var omp_neighborhood_search_method)" />
-    <param name="omp_num_threads" value="$(var omp_num_threads)" />
+    <param from="$(var param_file)" />
   </node>
 
 </launch>
diff --git a/localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -397,7 +397,7 @@ void NDTScanMatcher::callbackSensorPoints(
   {
     is_converged = false;
     ++skipping_publish_num;
-    std::cout << "Not Converged" << std::endl;
+    RCLCPP_WARN(get_logger(), "Not Converged");
   } else {
     skipping_publish_num = 0;
   }
@@ -532,12 +532,6 @@ void NDTScanMatcher::callbackSensorPoints(
   key_value_stdmap_["iteration_num"] = std::to_string(iteration_num);
   key_value_stdmap_["skipping_publish_num"] = std::to_string(skipping_publish_num);
 
-  std::cout << "------------------------------------------------" << std::endl;
-  std::cout << "align_time: " << align_time << "ms" << std::endl;
-  std::cout << "exe_time: " << exe_time << "ms" << std::endl;
-  std::cout << "trans_prob: " << transform_probability << std::endl;
-  std::cout << "iter_num: " << iteration_num << std::endl;
-  std::cout << "skipping_publish_num: " << skipping_publish_num << std::endl;
 }
 
 geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCarlo(
@@ -589,7 +583,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar
   auto best_particle_ptr = std::max_element(
     std::begin(particle_array), std::end(particle_array),
     [](const Particle & lhs, const Particle & rhs) {return lhs.score < rhs.score;});
-  // std::cout << "best score" << best_particle_ptr->score << std::endl;
 
   geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg;
   result_pose_with_cov_msg.header.frame_id = map_frame_;