feat(ndt_scan_matcher): add tolerance of initial pose (#408)

* feat(ndt_scan_matcher): add tolerance of initial pose

Signed-off-by: Yamato Ando <yamato.ando@gmail.com>

* move codes

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* modify the default value

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* change the variable names

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* ci(pre-commit): autofix

* fix typo

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* add depend fmt

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml

@@ -29,6 +29,12 @@
     # The number of particles to estimate initial pose
     initial_estimate_particles_num: 100
 
+    # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
+    initial_pose_timeout_sec: 1.0
+
+    # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
+    initial_pose_distance_tolerance_m: 10.0
+
     # neighborhood search method in OMP
     # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
     omp_neighborhood_search_method: 0

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -30,6 +30,12 @@
     # The number of particles to estimate initial pose
     initial_estimate_particles_num: 100
 
+    # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
+    initial_pose_timeout_sec: 1.0
+
+    # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
+    initial_pose_distance_tolerance_m: 10.0
+
     # neighborhood search method in OMP
     # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
     omp_neighborhood_search_method: 0

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -113,6 +113,13 @@ class NDTScanMatcher : public rclcpp::Node
     const std::string & target_frame, const std::string & source_frame,
     const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr);
 
+  bool validateTimeStampDifference(
+    const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
+    const double time_tolerance_sec);
+  bool validatePositionDifference(
+    const geometry_msgs::msg::Point & target_point,
+    const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_);
+
   void timerDiagnostic();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
@@ -154,6 +161,8 @@ class NDTScanMatcher : public rclcpp::Node
   std::string map_frame_;
   double converged_param_transform_probability_;
   int initial_estimate_particles_num_;
+  double initial_pose_timeout_sec_;
+  double initial_pose_distance_tolerance_m_;
   float inversion_vector_threshold_;
   float oscillation_threshold_;
   std::array<double, 36> output_pose_covariance_;

localization/ndt_scan_matcher/package.xml

@@ -8,6 +8,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <depend>diagnostic_msgs</depend>
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>ndt</depend>

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -104,6 +104,8 @@ NDTScanMatcher::NDTScanMatcher()
   map_frame_("map"),
   converged_param_transform_probability_(4.5),
   initial_estimate_particles_num_(100),
+  initial_pose_timeout_sec_(1.0),
+  initial_pose_distance_tolerance_m_(10.0),
   inversion_vector_threshold_(-0.9),
   oscillation_threshold_(10)
 {
@@ -169,6 +171,12 @@ NDTScanMatcher::NDTScanMatcher()
   initial_estimate_particles_num_ =
     this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_);
 
+  initial_pose_timeout_sec_ =
+    this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_);
+
+  initial_pose_distance_tolerance_m_ = this->declare_parameter(
+    "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_);
+
   std::vector<double> output_pose_covariance =
     this->declare_parameter<std::vector<double>>("output_pose_covariance");
   for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
@@ -433,9 +441,27 @@ void NDTScanMatcher::callbackSensorPoints(
     initial_pose_msg_ptr_array_, sensor_ros_time, initial_pose_old_msg_ptr,
     initial_pose_new_msg_ptr);
   popOldPose(initial_pose_msg_ptr_array_, sensor_ros_time);
-  // TODO(Tier IV): check pose_timestamp - sensor_ros_time
+
+  // check the time stamp
+  bool valid_old_timestamp = validateTimeStampDifference(
+    initial_pose_old_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_);
+  bool valid_new_timestamp = validateTimeStampDifference(
+    initial_pose_new_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_);
+
+  // check the position jumping (ex. immediately after the initial pose estimation)
+  bool valid_new_to_old_distance = validatePositionDifference(
+    initial_pose_old_msg_ptr->pose.pose.position, initial_pose_new_msg_ptr->pose.pose.position,
+    initial_pose_distance_tolerance_m_);
+
+  // must all validations are true
+  if (!(valid_old_timestamp && valid_new_timestamp && valid_new_to_old_distance)) {
+    RCLCPP_WARN(get_logger(), "Validation error.");
+    return;
+  }
+
   const auto initial_pose_msg =
     interpolatePose(*initial_pose_old_msg_ptr, *initial_pose_new_msg_ptr, sensor_ros_time);
+
   // enf of critical section for initial_pose_msg_ptr_array_
   initial_pose_array_lock.unlock();
 
@@ -688,3 +714,35 @@ bool NDTScanMatcher::getTransform(
   }
   return true;
 }
+
+bool NDTScanMatcher::validateTimeStampDifference(
+  const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
+  const double time_tolerance_sec)
+{
+  const double dt = std::abs((target_time - reference_time).seconds());
+  if (dt > time_tolerance_sec) {
+    RCLCPP_WARN(
+      get_logger(),
+      "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The "
+      "difference is %lf[sec] (the tolerance is %lf[sec]).",
+      reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec);
+    return false;
+  }
+  return true;
+}
+
+bool NDTScanMatcher::validatePositionDifference(
+  const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point,
+  const double distance_tolerance_m_)
+{
+  double distance = norm(target_point, reference_point);
+  if (distance > distance_tolerance_m_) {
+    RCLCPP_WARN(
+      get_logger(),
+      "Validation error. The distance from reference position to target position is %lf[m] (the "
+      "tolerance is %lf[m]).",
+      distance, distance_tolerance_m_);
+    return false;
+  }
+  return true;
+}