feat(imu_corrector): changed GyroBiasEstimator to use ndt pose instea…

…d of twist (autowarefoundation#956)

feat(imu_corrector): changed GyroBiasEstimator to use ndt pose instead of twist (autowarefoundation#5259)

* Implemented ndt pose based GyroBiasEstimator



* style(pre-commit): autofix

* Added missing includes



* FIxed default gyro_bias_threshold



* Restored gyro_bias_pub_, which had been deleted due to a mistake



* removed get_bias_std and anything else that is no longer needed as a result of remove



* Updated README.md



* Revert "Updated README.md"

This reverts commit f669c20.

* Updated README.md



* Added notes to README.md



---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Co-authored-by: SakodaShintaro <shintaro.sakoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

sensing/imu_corrector/README.md

@@ -51,10 +51,16 @@ Note that the node calculates bias from the gyroscope data by averaging the data
 
 ### Input
 
-| Name              | Type                                             | Description      |
-| ----------------- | ------------------------------------------------ | ---------------- |
-| `~/input/imu_raw` | `sensor_msgs::msg::Imu`                          | **raw** imu data |
-| `~/input/twist`   | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity |
+| Name              | Type                                            | Description      |
+| ----------------- | ----------------------------------------------- | ---------------- |
+| `~/input/imu_raw` | `sensor_msgs::msg::Imu`                         | **raw** imu data |
+| `~/input/pose`    | `geometry_msgs::msg::PoseWithCovarianceStamped` | ndt pose         |
+
+Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged.
+
+Currently, it is possible to use methods other than NDT as a `pose_source` for Autoware, but less accurate methods are not suitable for IMU bias estimation.
+
+In the future, with careful implementation for pose errors, the IMU bias estimated by NDT could potentially be used not only for validation but also for online calibration.
 
 ### Output
 
@@ -64,12 +70,11 @@ Note that the node calculates bias from the gyroscope data by averaging the data
 
 ### Parameters
 
-| Name                        | Type   | Description                                                                   |
-| --------------------------- | ------ | ----------------------------------------------------------------------------- |
-| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s]                                          |
-| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s]                                            |
-| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s]                                              |
-| `gyro_bias_threshold`       | double | threshold of the bias of the gyroscope [rad/s]                                |
-| `velocity_threshold`        | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] |
-| `timestamp_threshold`       | double | threshold of the timestamp diff between IMU and twist [s]                     |
-| `data_num_threshold`        | int    | number of data used to calculate bias                                         |
+| Name                                  | Type   | Description                                                                                 |
+| ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- |
+| `angular_velocity_offset_x`           | double | roll rate offset in imu_link [rad/s]                                                        |
+| `angular_velocity_offset_y`           | double | pitch rate offset imu_link [rad/s]                                                          |
+| `angular_velocity_offset_z`           | double | yaw rate offset imu_link [rad/s]                                                            |
+| `gyro_bias_threshold`                 | double | threshold of the bias of the gyroscope [rad/s]                                              |
+| `timer_callback_interval_sec`         | double | seconds about the timer callback function [sec]                                             |
+| `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] |

sensing/imu_corrector/config/gyro_bias_estimator.param.yaml

@@ -1,6 +1,5 @@
 /**:
   ros__parameters:
     gyro_bias_threshold: 0.0015 # [rad/s]
-    velocity_threshold: 0.0 # [m/s]
-    timestamp_threshold: 0.1 # [s]
-    data_num_threshold: 200 # [num]
+    timer_callback_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]

sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml

@@ -1,14 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="input_imu_raw" default="imu_raw"/>
-  <arg name="input_twist" default="twist"/>
+  <arg name="input_pose" default="pose"/>
   <arg name="output_gyro_bias" default="gyro_bias"/>
   <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
   <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
 
   <node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
     <remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
-    <remap from="~/input/twist" to="$(var input_twist)"/>
+    <remap from="~/input/pose" to="$(var input_pose)"/>
     <remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
     <param from="$(var gyro_bias_estimator_param_file)"/>
     <param from="$(var imu_corrector_param_file)"/>

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

@@ -14,58 +14,106 @@
 
 #include "gyro_bias_estimation_module.hpp"
 
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
 namespace imu_corrector
 {
-GyroBiasEstimationModule::GyroBiasEstimationModule(
-  const double velocity_threshold, const double timestamp_threshold,
-  const size_t data_num_threshold)
-: velocity_threshold_(velocity_threshold),
-  timestamp_threshold_(timestamp_threshold),
-  data_num_threshold_(data_num_threshold),
-  is_stopped_(false)
-{
-}
 
-void GyroBiasEstimationModule::update_gyro(
-  const double time, const geometry_msgs::msg::Vector3 & gyro)
+/**
+ * @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY)
+ */
+geometry_msgs::msg::Vector3 integrate_orientation(
+  const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list,
+  const geometry_msgs::msg::Vector3 & gyro_bias)
 {
-  if (time - last_velocity_time_ > timestamp_threshold_) {
-    return;
-  }
-  if (!is_stopped_) {
-    return;
-  }
-  gyro_buffer_.push_back(gyro);
-  if (gyro_buffer_.size() > data_num_threshold_) {
-    gyro_buffer_.pop_front();
+  geometry_msgs::msg::Vector3 d_rpy{};
+  double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds();
+  for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) {
+    double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds();
+
+    d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x);
+    d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y);
+    d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z);
+
+    t_prev = t_cur;
   }
+  return d_rpy;
 }
 
-void GyroBiasEstimationModule::update_velocity(const double time, const double velocity)
+/**
+ * @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the
+ * ground-truth pose from "pose_list".
+ */
+geometry_msgs::msg::Vector3 calculate_error_rpy(
+  const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
+  const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list,
+  const geometry_msgs::msg::Vector3 & gyro_bias)
 {
-  is_stopped_ = velocity <= velocity_threshold_;
-  last_velocity_time_ = time;
+  const geometry_msgs::msg::Vector3 rpy_0 =
+    tier4_autoware_utils::getRPY(pose_list.front().pose.orientation);
+  const geometry_msgs::msg::Vector3 rpy_1 =
+    tier4_autoware_utils::getRPY(pose_list.back().pose.orientation);
+  const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias);
+
+  geometry_msgs::msg::Vector3 error_rpy;
+  error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x);
+  error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y);
+  error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z);
+  return error_rpy;
 }
 
-geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const
+/**
+ * @brief update gyroscope bias based on a given trajectory data
+ */
+void GyroBiasEstimationModule::update_bias(
+  const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
+  const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list)
 {
-  if (gyro_buffer_.size() < data_num_threshold_) {
-    throw std::runtime_error("Bias estimation is not yet ready because of insufficient data.");
+  const double dt_pose =
+    (rclcpp::Time(pose_list.back().header.stamp) - rclcpp::Time(pose_list.front().header.stamp))
+      .seconds();
+  const double dt_gyro =
+    (rclcpp::Time(gyro_list.back().header.stamp) - rclcpp::Time(gyro_list.front().header.stamp))
+      .seconds();
+  if (dt_pose == 0 || dt_gyro == 0) {
+    throw std::runtime_error("dt_pose or dt_gyro is zero");
   }
 
-  geometry_msgs::msg::Vector3 bias;
-  bias.x = 0.0;
-  bias.y = 0.0;
-  bias.z = 0.0;
-  for (const auto & gyro : gyro_buffer_) {
-    bias.x += gyro.x;
-    bias.y += gyro.y;
-    bias.z += gyro.z;
+  auto error_rpy = calculate_error_rpy(pose_list, gyro_list, geometry_msgs::msg::Vector3{});
+  error_rpy.x *= dt_pose / dt_gyro;
+  error_rpy.y *= dt_pose / dt_gyro;
+  error_rpy.z *= dt_pose / dt_gyro;
+
+  gyro_bias_pair_.first.x += dt_pose * error_rpy.x;
+  gyro_bias_pair_.first.y += dt_pose * error_rpy.y;
+  gyro_bias_pair_.first.z += dt_pose * error_rpy.z;
+  gyro_bias_pair_.second.x += dt_pose * dt_pose;
+  gyro_bias_pair_.second.y += dt_pose * dt_pose;
+  gyro_bias_pair_.second.z += dt_pose * dt_pose;
+
+  geometry_msgs::msg::Vector3 gyro_bias;
+  gyro_bias.x = error_rpy.x / dt_pose;
+  gyro_bias.y = error_rpy.y / dt_pose;
+  gyro_bias.z = error_rpy.z / dt_pose;
+}
+
+/**
+ * @brief getter function for current estimated bias
+ */
+geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const
+{
+  geometry_msgs::msg::Vector3 gyro_bias_base;
+  if (
+    gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 ||
+    gyro_bias_pair_.second.z == 0) {
+    throw std::runtime_error("gyro_bias_pair_.second is zero");
   }
-  bias.x /= gyro_buffer_.size();
-  bias.y /= gyro_buffer_.size();
-  bias.z /= gyro_buffer_.size();
-  return bias;
+  gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x;
+  gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y;
+  gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z;
+  return gyro_bias_base;
 }
 
 }  // namespace imu_corrector

sensing/imu_corrector/src/gyro_bias_estimation_module.hpp

@@ -11,33 +11,30 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+
 #ifndef GYRO_BIAS_ESTIMATION_MODULE_HPP_
 #define GYRO_BIAS_ESTIMATION_MODULE_HPP_
 
-#include <geometry_msgs/msg/vector3.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 
 #include <deque>
+#include <utility>
+#include <vector>
 
 namespace imu_corrector
 {
 class GyroBiasEstimationModule
 {
 public:
-  GyroBiasEstimationModule(
-    const double velocity_threshold, const double timestamp_threshold,
-    const size_t data_num_threshold);
-  geometry_msgs::msg::Vector3 get_bias() const;
-  void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro);
-  void update_velocity(const double time, const double velocity);
+  GyroBiasEstimationModule() = default;
+  void update_bias(
+    const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
+    const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list);
+  geometry_msgs::msg::Vector3 get_bias_base_link() const;
 
 private:
-  const double velocity_threshold_;
-  const double timestamp_threshold_;
-  const size_t data_num_threshold_;
-  bool is_stopped_;
-  std::deque<geometry_msgs::msg::Vector3> gyro_buffer_;
-
-  double last_velocity_time_;
+  std::pair<geometry_msgs::msg::Vector3, geometry_msgs::msg::Vector3> gyro_bias_pair_;
 };
 }  // namespace imu_corrector