Fix odometry and acceleration processing pipeline #753

HaoguangYang · 2022-05-10T22:22:52Z

Fix odometry pose transform when odometry origin is different from base_link.
Fix covariance transformation when Odometry msg child_frame is not base_link.
Fix acceleration transformation, considering angular acceleration and centripetal acceleration.
Fix gravity removal for IMU with relative orientation (initial pose as [0,0,0,1]).

…on calculation.

…ideways

ayrton04 · 2022-05-11T08:39:20Z

Thank you for the PR.

Can you clarify some of these? Everything is marked as a fix, but these appear to be feature enhancements. Are you fixing any specific bugs that you can detail? Maybe just generally provide use cases that are solved by these changes.

Fix odometry pose transform when odometry origin is different from base_link

I need to go over the PR, but what will happen if the odometry data is not provided in a frame relative to base_link, e.g., if it comes from a motion capture system that has a fixed transform to the robot's world frame (direct transform from mocap_frame to odom)?

HaoguangYang · 2022-05-11T17:39:09Z

Items 1 and 2 were made in parallel with #728 , but with the additional enhancements of transforming the covariance correspondingly. To enable this feature, set parameter odomX_pose_use_child_frame to true. Background of the fix: A mobile robot has both a wheel odometer and an Intel RealSense T265 camera. The wheel odometer registers odom message at the geometry center of the robot (header.frame_id is odom and child_frame is base_link), while T265 reports odometry of itself -- header.frame_id is T265_odom and child_frame is T265_link. I want to fuse these two odom sources with EKF. With the T265 mounted on the edge of the robot, which is 30cm from base_link, a simple static transform from odom to T265_odom is not enough, as the size of the robot will couple rotation into the position components. Therefore, the differences in the two odom sources needs a proper transformation of the odom headers and child frames. Q: "what will happen if the odometry data is not provided in a frame relative to base_link, e.g., if it comes from a motion capture system that has a fixed transform to the robot's world frame (direct transform from mocap_frame to odom)?" A: This PR, as well as #728, fixes exactly that issue. E.g. in a motion capturing system, the origin of the robot pose is not necessarily the same as the origin of the wheel odometer on the robot, and the velocity reference frames (child_link) are not necessarily the same. Assume the mocap system provides odom message describing tf: mocap_odom->mocap_frame, the pose and velocity measured by the mocap on the child link (mocap_frame) should undergo the following transformation chain: mocap_frame->mocap_odom->odom->base_link to obtain the pose of base_link in odom. According to the definition of Odometry message, mocap_odom->odom is addressed by a static transform, and odom->base_link is provided by the robot odometer. Item 3 resolves the TODO in the prepareAcceleration function. The acceleration, after transformed to base_link, needs to remove the centripetal acceleration Omega x (Omega x r), where r is the translation vector. It also needs to remove the linear acceleration due to angular acceleration, beta x r, where beta is the time differential of Omega. Item 4 adds a flag to support removal of gravity on IMU sensors that are not mounted upright and yet reports a pose relative to its intialization pose. Examples include the IMU sensor on the Intel RealSense cameras, which reports the readings in the optical frame and with tf to base_link specified. The feature is enabled by setting the imuX_relative parameter to true. ✅TODO: I will populate comments on the changed sections and fix the linting issue. ✅TODO: A bug in the PR is found when removing gravity. I will push a follow-up commit.

…

________________________________ From: Tom Moore ***@***.***> Sent: Wednesday, May 11, 2022 4:39:31 AM To: cra-ros-pkg/robot_localization ***@***.***> Cc: Yang, Haoguang ***@***.***>; Author ***@***.***> Subject: Re: [cra-ros-pkg/robot_localization] Fix odometry and acceleration processing pipeline (PR #753) Thank you for the PR. Can you clarify some of these? Everything is marked as a fix, but these appear to be feature enhancements. Are you fixing any specific bugs that you can detail? Maybe just generally provide use cases that are solved by these changes. Fix odometry pose transform when odometry origin is different from base_link I need to go over the PR, but what will happen if the odometry data is not provided in a frame relative to base_link, e.g., if it comes from a motion capture system that has a fixed transform to the robot's world frame (direct transform from mocap_frame to odom)? ― Reply to this email directly, view it on GitHub<#753 (comment)>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/ACPMIPFEE2VBAPXOFN6ZFOTVJNW4HANCNFSM5VTACTRA>. You are receiving this because you authored the thread.Message ID: ***@***.***>

…atted code for linting checks.

ayrton04 · 2022-05-20T09:28:11Z

I haven't forgotten about this. Just need to make time for review. Thanks again for the PR and details.

ayrton04 · 2022-05-25T10:01:02Z

Q: "what will happen if the odometry data is not provided in a frame relative to base_link, e.g., if it comes from a motion capture system that has a fixed transform to the robot's world frame (direct transform from mocap_frame to odom)?"
A: This PR, as well as #728, fixes exactly that issue.

Hang on, I want to back up on this point. I'm sure you are aware of all of this, but for completeness, I want to go over it here.

r_l has always made the assumption that pose data will arrive in a frame that can be transformed to its world frame (e.g., odom) directly, without passing through the world_frame -> base_link_frame transform. So, for example, if I have an external motion capture system that generates my pose in the mocap frame, and I know the transform from mocap to odom, then I can publish the mocap->odom transform, and the EKF can use messages from the motion capture system directly. It will transform the messages from the mocap frame to the odom frame and fuse it correctly.

This has been supported from day 1; the reason I use this specific example is because I have done this exact thing. It also works if you want to fuse, e.g., Gazebo's "perfect" state messages into the robot's pose. But again, for externally-referenced world frames, this already worked.

Likewise, the package makes the assumption that any velocity data will be reported in a frame that can be transformed to base_link directly (i.e., without having to "pass through" the odom->base_link transform in the tree).

If a nav_msgs/Odometry message gets fed to the filter, the pose data gets transformed from the messages's frame_id into the filter's world_frame (e.g., odom), and the twist data gets transformed from the message's child_frame_id into the filter's base_link_frame (e.g., base_link). They are then fused directly.

One of the main shortcomings of r_l has always been the case where someone mounts a sensor with its own world frame to the robot itself. This gets complicated, because the sensor's offset from the robot's origin will be defined relative to base_link. So now we have pose data that is coming from a frame relative to base_link, and that pose data also has to get transformed using the transform that the EKF itself is producing (i.e., odom->base_link).

#728 solved this, I believe, by making it so that differential mode, when applied, would use the child_frame_id of the odometry message. This would allow us to define a base_link-><child_frame_id> transform. Then the differential mode would differentiate two consecutive pose measurements from that sensor (which would yield velocity in the sensor frame), and then that sensor-frame velocity would get transformed by the base_link-><child_frame_id> transform, and we'd get robot body-frame velocity.

ayrton04

OK, took a deeper read through this, and it looks sound. Thanks for the PR!

ayrton04 · 2022-05-25T10:18:25Z

src/ros_filter.cpp

+                                    state(StateMemberVyaw));
+      angular_acceleration_ = (newStateTwistRot - lastStateTwistRot_)/dt;
+      const Eigen::MatrixXd &cov = filter_.getEstimateErrorCovariance();
+      for (size_t i = 0; i < 3; i ++)


Just for consistency, it might make sense to change 3 to ORIENTATION_SIZE.

Suggested change

for (size_t i = 0; i < 3; i ++)

for (size_t i = 0; i < ORIENTATION_SIZE; i ++)

ayrton04 · 2022-05-25T11:24:51Z

I see #728 was actually broader in scope than I realised, and isn't just targeting differential mode.

…d to ros2 rolling

HaoguangYang · 2022-06-19T18:56:49Z

src/ros_filter.cpp

+                                    state(StateMemberVyaw));
+      angular_acceleration_ = (newStateTwistRot - lastStateTwistRot_)/dt;
+      const Eigen::MatrixXd &cov = filter_.getEstimateErrorCovariance();
+      for (size_t i = 0; i < 3; i ++)


Suggested change

for (size_t i = 0; i < 3; i ++)

for (size_t i = 0; i < ORIENTATION_SIZE; i ++)

HaoguangYang · 2022-06-19T18:57:18Z

src/ros_filter.cpp

+      const Eigen::MatrixXd &cov = filter_.getEstimateErrorCovariance();
+      for (size_t i = 0; i < 3; i ++)
+      {
+        for (size_t j = 0; j < 3; j ++)


Suggested change

for (size_t j = 0; j < 3; j ++)

for (size_t j = 0; j < ORIENTATION_SIZE; j ++)

* compiling version of commit #753 and #728 ported to ros2 rolling * format fixes * fix time source disagreement by converting to seconds beforehand, append parameter usage, fix linting * fix linting and uncrustify

rokusottervanger · 2023-11-24T11:25:12Z

I came here because I suddenly see drift in my odometry state because my IMU is at a small but non-negligable angle (maybe the robot is at that angle, or the imu is not mounted perfectly straight.

Looking at this PR, I find it odd that:

There's suddenly math being performed on the state in the ros_filter layer (calculating the angular acceleration by means of differentiation)?
There's suddenly state being held outside of the filter_'s state (specifically the angular acceleration)?
The filter assumes a constant linear acceleration and constant angular velocity model, but now the angular acceleration is also used?

HaoguangYang added 4 commits March 7, 2022 01:11

Unifies odom frame before transforming odom pose. Corrects accelerati…

ac7dfc2

…on calculation.

support gravity removal of relatively-oriented IMU, and IMU mounted s…

0d4d031

…ideways

fix pull request on odometry child frame transform

a3e333d

Merge branch 'cra-ros-pkg-noetic-devel' into noetic-devel

8533de8

HaoguangYang added 2 commits May 11, 2022 17:55

corrected gravity removal / acceleration transformation order. reform…

1f6415f

…atted code for linting checks.

fix roslint issues

32c8a5e

ayrton04 approved these changes May 25, 2022

View reviewed changes

ayrton04 merged commit d428330 into cra-ros-pkg:noetic-devel May 25, 2022

HaoguangYang added a commit to HaoguangYang/robot_localization that referenced this pull request Jun 19, 2022

compiling version of commit cra-ros-pkg#753 and cra-ros-pkg#728 porte…

9d7f7c5

…d to ros2 rolling

HaoguangYang commented Jul 8, 2022

View reviewed changes

HaoguangYang mentioned this pull request Jul 14, 2022

Port PR #753 and #728 to ROS2 version #765

Merged

rokusottervanger mentioned this pull request Nov 24, 2023

Odometry drift caused by acceleration drift removal in relative mode #846

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix odometry and acceleration processing pipeline #753

Fix odometry and acceleration processing pipeline #753

HaoguangYang commented May 10, 2022

ayrton04 commented May 11, 2022

HaoguangYang commented May 11, 2022 via email •

edited

Loading

ayrton04 commented May 20, 2022

ayrton04 commented May 25, 2022 •

edited

Loading

ayrton04 left a comment

ayrton04 May 25, 2022

HaoguangYang Jun 19, 2022

ayrton04 commented May 25, 2022

HaoguangYang Jun 19, 2022

HaoguangYang Jun 19, 2022

rokusottervanger commented Nov 24, 2023 •

edited

Loading

	for (size_t i = 0; i < 3; i ++)
	for (size_t i = 0; i < ORIENTATION_SIZE; i ++)

	for (size_t j = 0; j < 3; j ++)
	for (size_t j = 0; j < ORIENTATION_SIZE; j ++)

Fix odometry and acceleration processing pipeline #753

Fix odometry and acceleration processing pipeline #753

Conversation

HaoguangYang commented May 10, 2022

ayrton04 commented May 11, 2022

HaoguangYang commented May 11, 2022 via email • edited Loading

ayrton04 commented May 20, 2022

ayrton04 commented May 25, 2022 • edited Loading

ayrton04 left a comment

Choose a reason for hiding this comment

ayrton04 May 25, 2022

Choose a reason for hiding this comment

HaoguangYang Jun 19, 2022

Choose a reason for hiding this comment

ayrton04 commented May 25, 2022

HaoguangYang Jun 19, 2022

Choose a reason for hiding this comment

HaoguangYang Jun 19, 2022

Choose a reason for hiding this comment

rokusottervanger commented Nov 24, 2023 • edited Loading

HaoguangYang commented May 11, 2022 via email •

edited

Loading

ayrton04 commented May 25, 2022 •

edited

Loading

rokusottervanger commented Nov 24, 2023 •

edited

Loading