0
$\begingroup$

Rosanswers logo

I'm fusing GPS, Visual Odometry and IMU. After reading the documentations with some try and error tests, finally I can get a reasonable output.

I put the frame_id of my Visual Odometry output as odom. I put the frame_id of my IMU as base_link. The GPS data is transformed to odom frame using navsat_transform_node and all three sensors are fused in ekf_node. Because Visual Odometry and GPS both produce x,y,z in state vector, I put differential mode to true for visual odometry as it is less accurate than GPS. I want you to explain me if this configuration is wrong. (I will send my launch file if you need)

Anyway

I'm confused about coordinate frames. The first time I read about the package, I though that it can handle multiple sensors in their own coordinate frames. I thought that It includes some sort of self-calibration. But Now, what I get is that the package only can produce /tf message between odom and base_link frames.

Is this true? I mean if I want to fuse a sensor in another frame, say odom_b, then do I have to provide /tf from odom_b to odom?

Next question: I know that visual odometry becomes less and less accurate over time. When I publish its poses, I just put the covariance of current measure and don't account for previous covariances. Does the filter accumulates those covariances it self?

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">

  <param name="magnetic_declination_radians" value="0"/>

  <param name="yaw_offset" value="0"/>
  <param name="zero_altitude" value="true"/>

  <remap from="/imu/data" to="/imu_with_cov" />
  <remap from="/gps/fix" to="/fix" />
  <remap from="/odometry/filtered" to="/odometry/filtered" />

</node>

<node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">

<!-- ======== STANDARD PARAMETERS ======== -->

<param name="frequency" value="10"/>

<param name="sensor_timeout" value="0.1"/>

<param name="two_d_mode" value="true"/>

<!-- Defaults to "odom" if unspecified -->
<param name="odom_frame" value="odom"/>
<!-- Defaults to "base_link" if unspecified -->
<param name="base_link_frame" value="imu_link"/>
<!-- Defaults to the value of "odom_frame" if unspecified -->
<param name="world_frame" value="odom"/>

<param name="transform_time_offset" value="0.0"/>

<param name="odom0" value="/vo"/>
<param name="odom1" value="/odometry/gps"/>
<param name="imu0" value="/imu_with_cov"/>

<rosparam param="odom0_config">[true, true, true,
                                false, false, false,
                                false, false, false,
                                false, false, false,
                                false, false, false]</rosparam>

<rosparam param="odom1_config">[true, true, true,
                                false, false, false,
                                false, false, false,
                                false, false, false,
                                false, false, false]</rosparam>

<rosparam param="imu0_config">[false, false, false,
                               true, true, true,
                               false, false, false,
                               false, false, false,
                               false, false, false]</rosparam>

<param name="odom0_differential" value="true"/>
<param name="odom1_differential" value="false"/>
<param name="imu0_differential" value="false"/>

<param name="odom0_relative" value="true"/>
<param name="odom1_relative" value="true"/>
<param name="imu0_relative" value="true"/>

<param name="print_diagnostics" value="true"/>

<!-- ======== ADVANCED PARAMETERS ======== -->

<param name="odom0_queue_size" value="5"/>
<param name="odom1_queue_size" value="50"/>
<param name="imu0_queue_size" value="50"/>

<param name="debug"           value="false"/>

<param name="debug_out_file"  value="debug_ekf_localization.txt"/>
<node pkg="my_mono_vo" type="monocularVO" name="monocularVO" respawn="true">

</node>

EDIT1:

Hi Tom

So many many thanks for your explanations

I thought that when I trun on two_d_mode, even if I set Z or roll and pitch to true, the filter ignores it.

I set those parameters to false

I also removed orientation information from my visual odometry message as they were wrong. Before this, I thought that when I turn off orientations in sensor config, the filter ignores the orientations even if they are published.

Now I publish my visual odometry X and Y in odom frame and use it as velocity by turning on differential mode. Results are better and fluctuations have been reduced. But they still happen sometimes. Isn't there any parameter to smoothen filter's output?

I use UKF as it produces more accurate ad stable results over EKF (in my experiences)

Edit2 (in response to Tom's answer to Edit 1):

I don't think that's because of orientation. We know that GPS is fused as an odometry message. Just like my visual odometry, It contains only x and y. No orientation is involved. If you are right, then I should produce orientation for GPS too!

When I set false for all orientation parameters, I expect the filter to not to use them. And this must happen before delta calculation.

Edit3 (in response to Tom's answer to Edit 2):

You are all right. I corrected my yaw angles and everythings works fine now. Results are smooth and meaningful. The only point is that the UKF output is exactly overlaid on the output of navsat_transform_node. I guess this is because of low covariance of GPS. The filter totally relies on it and it seems that the speeds obtained from visual odometry are somehow ignored. However, when I remove GPS, and fuse only IMU and visual odometry, I can see a refined output from UKF. That means that the filter works. I think the remainder is a good covariance tunning.

Thank you Tom. You saved me


Originally posted by ZanaZak on ROS Answers with karma: 58 on 2018-01-13

Post score: 0


Original comments

Comment by Tom Moore on 2018-01-14:
Please post your full launch file, and a sample message for every input (e.g., your NavSatFix message, your IMU message, and your visual odometry message).

Comment by Tom Moore on 2018-01-17:
Great! Would you mind accepting the answer below? Just click the checkmark.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I put the frame_id of my Visual Odometry output as odom. I put the frame_id of my IMU as base_link. The GPS data is transformed to odom frame using navsat_transform_node and all three sensors are fused in ekf_node. Because Visual Odometry and GPS both produce x,y,z in state vector, I put differential mode to true for visual odometry as it is less accurate than GPS. I want you to explain me if this configuration is wrong.

It's always very helpful to post launch files AND sample input messages (preferably collected when your robot is moving) for questions about r_l. Without them, I can't really comment with any certainty about what your optimal setup is. This leads to two questions:

  1. Does your visual odometry produce velocity data, i.e., is the Twist section of the message filled out? If it does, then set X, Y, and Z to false in the config file for visual odometry, and set , , and Ż (the velocities for XYZ) to true.

If your visual odometry only produces pose data, then yes, set the visual odometry differential parameter to true. 2. How is your IMU mounted? If it's upside-down or sideways or rotated on its side, then having its frame as base_link is not going to work well.

I'm confused about coordinate frames. The first time I read about the package, I though that it can handle multiple sensors in their own coordinate frames. I thought that It includes some sort of self-calibration. But Now, what I get is that the package only can produce /tf message between odom and base_link frames.

I would thoroughly read REP-105 to start. The package can handle sensors in their own coordinate frames, certainly, but the package's job is to provide a transform from your world_frame to your base_link_frame in your configuration file. In your case, it's producing odom->base_link. That is the only transform that r_l produces.

However, it still works with data in any coordinate frame, provided you tell it how to transform from that coordinate frame to your world_frame or base_link_frame. Let's say, for example, that your IMU is mounted upside-down on your robot and is placed on the back end of the robot. This means that your IMU's coordinate frame is not base_link, since it's not at your robot's origin. That means you must provide, via URDF + robot_state_publisher or via a tf static_transform_publisher, a transform from base_link->imu, and then set the frame_id in your IMU messages to imu. When r_l receives the IMU message, it will see that its frame_id is IMU, and will try to transform it to base_link.

So: if you are fusing pose data, you need to make sure there's a transform from your sensor message's frame_id to your world_frame. If you are fusing velocity data, then you need to make sure that there's a transform from your sensor's child_frame_id (for nav_msgs/Odometry messages) or frame_id (for geometry_msgs/TwistWithCovarianceStamped) messages to your base_link_frame. IMU data is a special case; just make sure there's a transform from the message's frame_id to your base_link_frame. These are not the responsibility of r_l; it can't automatically determine transforms between coordinate frames with no prior knowledge.

Next question: I know that visual odometry becomes less and less accurate over time. When I publish its poses, I just put the covariance of current measure and don't account for previous covariances. Does the filter accumulates those covariances it self?

Visual odometry, like wheel encoder odometry, is subject to drift over time, in that it's integrating lots of relative pose information, without any fixed reference/ground truth. As such, the covariance in visual odometry data should grow without bound. r_l does not support relative pose measurements, where the measurement at time t2 is relative to the measurement at time t1. In this case, your visual odometry node should be increasing its covariance as every message is generated.

However, you are using differential mode, so you can get away with leaving it as-is. Even so, if I were you, I'd start publishing the velocities from your visual odometry and get rid of the the pose data/differential mode in your EKF config. Just remember that velocities need to be published in the robot's base_link_frame frame, so if a robot is moving straight ahead, then it will always have +X velocity, no matter which way it is facing.

EDIT after @ZanaZak sent me bag files:

There are a bunch of issues here, but only one is really responsible for the issue you're seeing. I'll start with that one.

  1. Your visual odometry data is in its own coordinate frame, and you have not (a) set that coordinate frame in the header of the vo message, and (b) you have not provided a transform from the vo frame into the odom frame. Did you try using rviz to look at your VO data? Look at this rviz screen capture:

image description

Red is the EKF output, and blue is the visual odometry output. You'll notice that the visual odometry starts out level, but then slowly starts pointing down into the ground. So, when the EKF differentiates two vo poses, it sees velocity in the body-frame Y axis, rather than in the body-frame X axis, as it should. If you look at your VO data, the quaternion confirms this:

    pose: 
      pose: 
        position: 
          x: -44.780964
          y: -138.77340
          z: -12.447221
        orientation: 
          x: 0.01812456
          y: 0.76774616
          z: 0.06945744
          w: 0.63672049

That corresponds to Euler angles of (-2.706, 1.371, 2.830). Note that pitch is nearly pi/2, which means it's facing straight down. You need to either fix your visual odometry output so that the orientations are correct, or provide a transform from the visual odometry frame to the odom frame, and then put the visual odometry frame_id into the visual odometry messages. Whatever you do, I recommend using visualizations to verify that your inputs conform to standards before fusing them in the EKF.

  1. You have two_d_mode set to true, but are fusing Z from your GPS data and your visual odometry data, and you are fusing roll and pitch from the IMU. If you want to operate in 2D, then there's no point in fusing the 3D variables, as they get ignored. If you want a 3D state estimate, turn off two_d_mode.

  2. This is not critical, but you have the EKF generating an odom->imu_link transform. If I were you, I'd generate an odom->base_link transform, and then use a static_transform_publisher to generate the base_link->imu_link transform, even if that transform has no linear or rotational offsets.

TL;DR it's critical that you check every input source individually and make sure it conforms to standards before you fuse it in the EKF. Then try fusing just that input into the EKF, and see if it does what you expect, then add a new sensor (after verifying its data as well).

EDIT 2 in response to update:

I also removed orientation information from my visual odometry message as they were wrong. Before this, I thought that when I turn off orientations in sensor config, the filter ignores the orientations even if they are published.

This won't work, and is the reason your output is still not smooth. See below.

Now I publish my visual odometry X and Y in odom frame and use it as velocity by turning on differential mode. Results are better and fluctuations have been reduced. But they still happen sometimes. Isn't there any parameter to smoothen filter's output?

It would be great if you could post bags (links to DropBox are fine) or even a video (also, show me specifically which params you updated), but in this case, the problem is that you have no orientation data in your VO messages.

Differential mode works by taking the delta between two consecutive measurements and turning that into a velocity. That delta must have orientation data if your robot was really turning. (By the way, two_d_mode gets applied after the differential calculation). By removing orientation data from your VO data and still using it in differential mode, you have effectively said that your robot is now holonomic, i.e., it can move instantaneously in any direction, without having to turn. You are telling the filter that your robot never turns. Even if this is true, it's not the way the robot was really moving.

The good news is that I think you just have some axes swapped in your VO orientation. When the real robot turned to the right (-yaw, which is negative rotation around the Z axis), the VO data turned into the ground.

In any case, don't try to use EKF params to skirt around bad data. As the saying goes, garbage in, garbage out. Take the time to fix your inputs and make sure they conform to standards, then try again.

EDIT 3 in response to Edit 2 above:

I don't think that's because of orientation. We know that GPS is fused as an odometry message. Just like my visual odometry, It contains only x and y. No orientation is involved. If you are right, then I should produce orientation for GPS too!

No, it is because of orientation. You are forgetting that navsat_transform_node is converting GPS pose data to EKF-world-frame pose data, and it's using your robot's initial orientation to ensure that they are consistent.

Differential mode, which you are using for your vo data, is converting poses to velocities. Consider a robot that moves from a pose of (1, 1) with an orientation of 0.5 radians to a pose of (1.1, 1.1) with an orientation of 0.6 radians:

tf2::Transform pose1;
pose1.setOrigin(tf2::Vector3(1, 1, 0.0));
tf2::Quaternion quat1;
quat1.setRPY(0.0, 0.0, 0.5);
pose1.setRotation(quat1);

tf2::Transform pose2;
pose2.setOrigin(tf2::Vector3(1.1, 1.1, 0.0));
tf2::Quaternion quat2;
quat2.setRPY(0.0, 0.0, 0.6);
pose2.setRotation(quat2);

tf2::Transform delta_pose = pose1.inverse() * pose2;
ROS_INFO_STREAM("delta pose is " << delta_pose);

The output is this:

[1516093598.222760800] [ INFO] [/ekf_navigation_node]: delta pose is
Origin: (0.13570081004945766523 0.039815702328616975336 0)
Rotation (RPY): (0, -0, 0.099999999999999755751)

Note that the X value is much larger than the Y value, even though their position delta, without orientation, is (0.1, 0.1):

tf2::Transform pose1;
pose1.setOrigin(tf2::Vector3(1, 1, 0.0));
tf2::Quaternion quat1;
quat1.setRPY(0.0, 0.0, 0.0);
pose1.setRotation(quat1);

tf2::Transform pose2;
pose2.setOrigin(tf2::Vector3(1.1, 1.1, 0.0));
tf2::Quaternion quat2;
quat2.setRPY(0.0, 0.0, 0.0);
pose2.setRotation(quat2);

tf2::Transform delta_pose = pose1.inverse() * pose2;
ROS_INFO_STREAM("delta pose is " << delta_pose);

Output is:

[1516093766.275570279] [ INFO] [/ekf_navigation_node]: delta pose is
Origin: (0.10000000000000008882 0.10000000000000008882 0)
Rotation (RPY): (0, -0, 0)

The upshot of this is that you are giving your robot false linear motion in the body-frame Y axis. It's going to cause it to move sideways in the output when your robot turns.

When I set false for all orientation parameters, I expect the filter to not to use them. And this must happen before delta calculation.

I'm sorry that the package does not behave as you expect, but delta calculation happens first, and that is by design.


Originally posted by Tom Moore with karma: 13689 on 2018-01-14

This answer was ACCEPTED on the original site

Post score: 4


Original comments

Comment by gvdhoorn on 2018-01-14:\

you must provide, via URDF/robot_state_publisher

shouldn't that be: "via URDF + robot_state_publisher"? URDF on its own doesn't do much, and neither does robot_state_publisher. They're going to need each other :)

Comment by Tom Moore on 2018-01-14:
Sorry, yes, I was using the / to treat them as a group. Will clarify, TY.

Comment by gvdhoorn on 2018-01-15:
Can't upvote this more than once, but +100 for analysing these posts with such detail every time @Tom Moore.

Seems sensor fusion is not easy, and ROS new comers and experienced users need quite some help.

Comment by Tom Moore on 2018-01-15:
Thanks, @gvdhoorn. I should really take these questions as an opportunity to improve the wiki. I also wish ROS Answers were easier to search, because I find I often answer similar sorts of questions. I know you can use Google, but I wish that the native search were as powerful.

Comment by gvdhoorn on 2018-01-15:
re: update wiki: yes, a faq might be an idea? Would save you from some typing perhaps. Although I've experienced that you can't always just point to a faq item unfortunately.

re: search: ros-infrastructure/answers.ros.org#155.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.