Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 10 commits into from
Oct 17, 2023
Merged

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

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
cfcb764
Implemented ndt pose based GyroBiasEstimator
SakodaShintaro Oct 10, 2023
f5049e1
style(pre-commit): autofix
pre-commit-ci[bot] Oct 10, 2023
105ddec
Added missing includes
SakodaShintaro Oct 10, 2023
b012405
FIxed default gyro_bias_threshold
SakodaShintaro Oct 12, 2023
b762f90
Restored gyro_bias_pub_, which had been deleted due to a mistake
SakodaShintaro Oct 12, 2023
482b3cc
removed get_bias_std and anything else that is no longer needed as a …
SakodaShintaro Oct 12, 2023
f669c20
Updated README.md
SakodaShintaro Oct 13, 2023
7145120
Revert "Updated README.md"
SakodaShintaro Oct 16, 2023
53bbd50
Updated README.md
SakodaShintaro Oct 16, 2023
9babe99
Added notes to README.md
SakodaShintaro Oct 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 13 additions & 13 deletions sensing/imu_corrector/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,10 @@ 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 |
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


### Output

Expand All @@ -64,12 +64,12 @@ 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] |
| `data_num_threshold` | int | number of data used to calculate bias |
4 changes: 2 additions & 2 deletions sensing/imu_corrector/config/gyro_bias_estimator.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/**:
ros__parameters:
gyro_bias_threshold: 0.0015 # [rad/s]
velocity_threshold: 0.0 # [m/s]
timestamp_threshold: 0.1 # [s]
timer_callback_interval_sec: 0.5 # [sec]
straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
data_num_threshold: 200 # [num]
kminoda marked this conversation as resolved.
Show resolved Hide resolved
4 changes: 2 additions & 2 deletions sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)"/>
Expand Down
160 changes: 124 additions & 36 deletions sensing/imu_corrector/src/gyro_bias_estimation_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,58 +14,146 @@

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

/**
* @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)
{
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_gyro(
const double time, const geometry_msgs::msg::Vector3 & gyro)
/**
* @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)
{
if (time - last_velocity_time_ > timestamp_threshold_) {
return;
}
if (!is_stopped_) {
return;
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;
}

GyroBiasEstimationModule::GyroBiasEstimationModule(const size_t data_num_threshold)
: data_num_threshold_(data_num_threshold)
{
}

/**
* @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)
{
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");
}
gyro_buffer_.push_back(gyro);
if (gyro_buffer_.size() > data_num_threshold_) {
gyro_buffer_.pop_front();

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;
gyro_bias_deque_.push_back(gyro_bias);
if (gyro_bias_deque_.size() > data_num_threshold_) {
gyro_bias_deque_.pop_front();
}
}

void GyroBiasEstimationModule::update_velocity(const double time, const double velocity)
/**
* @brief getter function for current estimated bias
*/
geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const
{
is_stopped_ = velocity <= velocity_threshold_;
last_velocity_time_ = time;
geometry_msgs::msg::Vector3 gyro_bias_base;
if (
gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 ||

Check warning on line 118 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Conditional 

GyroBiasEstimationModule::get_bias_base_link has 1 complex conditionals with 2 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
gyro_bias_pair_.second.z == 0) {
throw std::runtime_error("gyro_bias_pair_.second is zero");
}
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;
}

geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const
/**
* @brief getter function for current standard deviation of estimated bias
*/
geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_std() const

Check warning on line 131 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

Codecov / codecov/patch

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp#L131 

Added line #L131 was not covered by tests
{
if (gyro_buffer_.size() < data_num_threshold_) {
throw std::runtime_error("Bias estimation is not yet ready because of insufficient data.");
}
auto calculate_std_mean_const = [](const std::vector<double> & v, const double mean) {

Check warning on line 133 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

Codecov / codecov/patch

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp#L133 

Added line #L133 was not covered by tests
if (v.size() == 0) {
return 0.0;
}

double error = 0;
for (const double & t : v) {
error += pow(t - mean, 2);

Check warning on line 140 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

Codecov / codecov/patch

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp#L140 

Added line #L140 was not covered by tests
}
return std::sqrt(error / v.size());

Check warning on line 142 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

Codecov / codecov/patch

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp#L142 

Added line #L142 was not covered by tests
};

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;
std::vector<double> stddev_bias_list_x, stddev_bias_list_y, stddev_bias_list_z;
for (const auto & e : gyro_bias_deque_) {
stddev_bias_list_x.push_back(e.x);
stddev_bias_list_y.push_back(e.y);
stddev_bias_list_z.push_back(e.z);
}
bias.x /= gyro_buffer_.size();
bias.y /= gyro_buffer_.size();
bias.z /= gyro_buffer_.size();
return bias;
geometry_msgs::msg::Vector3 mean = get_bias_base_link();
geometry_msgs::msg::Vector3 stddev_bias;
stddev_bias.x = calculate_std_mean_const(stddev_bias_list_x, mean.x);
stddev_bias.y = calculate_std_mean_const(stddev_bias_list_y, mean.y);

Check warning on line 154 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

Codecov / codecov/patch

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp#L153-L154 

Added lines #L153 - L154 were not covered by tests
stddev_bias.z = calculate_std_mean_const(stddev_bias_list_z, mean.z);
return stddev_bias;

Check warning on line 156 in sensing/imu_corrector/src/gyro_bias_estimation_module.cpp

View check run for this annotation

Codecov / codecov/patch

sensing/imu_corrector/src/gyro_bias_estimation_module.cpp#L156 

Added line #L156 was not covered by tests
}

} // namespace imu_corrector
26 changes: 13 additions & 13 deletions sensing/imu_corrector/src/gyro_bias_estimation_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,33 +11,33 @@
// 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);
explicit GyroBiasEstimationModule(const size_t data_num_threshold);
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;
geometry_msgs::msg::Vector3 get_bias_std() const;
kminoda marked this conversation as resolved.
Show resolved Hide resolved

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::deque<geometry_msgs::msg::Vector3> gyro_bias_deque_;
std::pair<geometry_msgs::msg::Vector3, geometry_msgs::msg::Vector3> gyro_bias_pair_;
};
} // namespace imu_corrector

Expand Down
Loading
Loading