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

[SakodaShintaro]

Description

Before : Use twist to determine whether ego-vehicle is stopping, and calculate imu bias by comparing it to 0 while it is stopping.

After: Use ndt pose to determine whether ego-vehicle is curving, and calculate imu bias by comparing it to ndt pose while it is not curving.

Related link

• feat(imu_launch): fixed gyro_bias_estimator input sample_sensor_kit_launch#78
• feat(imu_launch): fixed gyro_bias_estimator input tier4/aip_launcher#180
• feat(imu_corrector): add gyro_bias_estimator tier4/awsim_sensor_kit_launch#10

Tests performed

It has been confirmed that it works as expected with sample rosbag.

The red dashed line indicates the range extended from the orange line, which is the calibration value, by gyro_bias_threshold.

If it is within the red dashed line range, the IMU bias is considered normal.

For TIER IV's internal rosbags, the result of the IMU bias variation detection function for data taken on a day not long after calibration is as follows.

For data taken on a day when more time has passed since calibration, the result is as follows.

Effects on system behavior

Improves performance of IMU bias variation detection.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[kminoda]

Thank you very much! Let me review until tomorrow 🙏

[codecov]

Codecov Report

Attention: 83 lines in your changes are missing coverage. Please review.

Comparison is base (c508aa7) 14.78% compared to head (9babe99) 14.78%.
Report is 50 commits behind head on main.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #5259   +/-   ##
=======================================
  Coverage   14.78%   14.78%           
=======================================
  Files        1644     1644           
  Lines      113948   114020   +72     
  Branches    35160    35179   +19     
=======================================
+ Hits        16850    16862   +12     
- Misses      78143    78181   +38     
- Partials    18955    18977   +22     

Flag	Coverage Δ		*Carryforward flag
differential	21.94% <35.65%> (?)
total	14.76% <ø> (-0.03%)	⬇️	Carriedforward from f669c20

*This pull request uses carry forward flags. Click here to find out more.

Files	Coverage Δ
.../imu_corrector/src/gyro_bias_estimation_module.hpp	0.00% <ø> (ø)
.../imu_corrector/src/gyro_bias_estimation_module.cpp	75.55% <74.41%> (-13.74%)	⬇️
...orrector/test/test_gyro_bias_estimation_module.cpp	51.28% <42.42%> (-26.00%)	⬇️
sensing/imu_corrector/src/gyro_bias_estimator.cpp	0.00% <0.00%> (ø)

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[kminoda]

Thank you! I left some comments 🙏

[kminoda]

Would you create a PR for other sensor kit packages as well? (e.g. aip_launcher)

[SakodaShintaro]

Would you create a PR for other sensor kit packages as well? (e.g. aip_launcher)

I have also created this pull request.
tier4/aip_launcher#180

[kminoda]

One final comment 🙏

[kminoda]

Would you state here something like as follows as a side note below?

Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged.

[SakodaShintaro]

That's a very good. In fact, one needs to be careful when using a pose other than NDT, or when using NDT in an environment where its accuracy will be low.
Currently, it is only output to diagnostics, so there may not be a need to be overly cautious. However, caution is needed when changing the IMU value based on the estimate obtained here for online calibration.

Fixed in f669c20

[kminoda]

Thanks! But what I meant was to put it as a side note maybe as a footer. The description ndt pose itself was OK.

So something like

### Input

| 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.

[kminoda]

However, caution is needed when changing the IMU value based on the estimate obtained here for online calibration.

Correct. That's one of the main concern for using NDT for estimation, which makes some kind of a new closed loop in Autoware. So would like to add some warnings for future developers.

[SakodaShintaro]

I created a pull request to launch gyro_bias_estimator in pose mode in AWSIM as well, but I can not seem to add reviewers.

tier4/awsim_sensor_kit_launch#10

[kminoda]

LGTM!