Ask Your Question
0

robot_localization drift

asked 2015-09-01 01:56:12 -0600

K7 gravatar image

updated 2015-09-01 22:03:09 -0600

I am trying to use robot_localization to fuse IMU with PTAM but I am getting some major drift. The error seems to be compounding.

At first I thought that the axes were not aligned correctly (which still might be the case) but now I am wondering if the PTAM data is being ignored or not factored in.

IMU message:

---
header: 
  seq: 20112
  stamp: 
    secs: 1441090194
    nsecs: 749087915
  frame_id: usb_cam
pose: 
  pose: 
    position: 
      x: 31.8292968996
      y: 15.1699469361
      z: -236.728859078
    orientation: 
      x: 0.726365842025
      y: 0.0331459598161
      z: -0.0598464211576
      w: 0.683895031977
  covariance: [0.0039018500775288606, 0.0027630803033894093, 0.007831431075466348, 0.00037953225698867527, 0.0012029410262677585, 0.0008225141417525416, 0.0027630803033894093, 0.0022154257522649786, 0.005748820203067354, 0.0004981530611692311, 0.0008370959001320362, 0.00021214889413358968, 0.007831431075466348, 0.005748820203067352, 0.016284114101305945, 0.0008166119036448137, 0.002352189342863207, 0.0015354683751600184, 0.0003795322569886803, 0.0004981530611692337, 0.0008166119036448272, 0.00044415715572847697, 0.00014435743999326682, 0.0004755561972274128, 0.0012029410262677585, 0.0008370959001320371, 0.0023521893428632076, 0.00014435743999326628, 0.0005786599694969616, 8.747369640226204e-05, 0.0008225141417525225, 0.00021214889413362665, 0.0015354683751599768, 0.00047555619722741335, 8.747369640224455e-05, 0.0014638212613628672]
---

IMU message (time delay between last message):

---
header: 
  seq: 415116
  stamp: 
    secs: 1441090283
    nsecs: 586467584
  frame_id: fcu
orientation: 
  x: 0.0348078489314
  y: 0.0336684433503
  z: 0.620517126138
  w: 0.782696202715
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity: 
  x: 0.000315623357892
  y: -0.000284243375063
  z: 0.000455724075437
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: -0.084431245923
  y: 0.915327310562
  z: 9.66289806366
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
---

tf launch file:

<launch>
    <node pkg="tf" type="static_transform_publisher" name="odom" args="-0.5 0 0 1.5707 0 1.5707 odom usb_cam 100" />
    <node pkg="tf" type="static_transform_publisher" name="base_link" args="0 0 0 0 0 0 base_link fcu 100" />
</launch>

And the ekf launch file:

<launch>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

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

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

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

  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>

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

  <param name="pose0" value="/vslam/pose"/>
  <param name="imu0" value="/mavros/imu/data"/>

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

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


  <param name="pose0_differential" value="true"/>
  <param name="imu0_differential" value="false"/>

  <param name="pose0_relative" value="false"/>
  <param name="imu0_relative" value="true"/>

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

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

</node>

</launch>

The robot_localization message:

    ---
header: 
  seq: 75
  stamp: 
    secs: 1441104536
    nsecs: 404455893
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.0327968532627
      y: 0.0107415590331
      z: 0.0672586918467
    orientation: 
      x: 0.000269673528914
      y: -9.62085270466e-05
      z: 0.000620642648884
      w: 0.999999766411
  covariance: [0.25178859948463683, -4.971036727512653e-06, -3.834708230879292e-05, 5.617159792226774e-07 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you show me a sample output message (from the EKF)?

Tom Moore gravatar imageTom Moore ( 2015-09-01 05:43:30 -0600 )edit

Sure. I'll update the question now.

K7 gravatar imageK7 ( 2015-09-01 05:50:23 -0600 )edit

@Tom Moore sample output is there now. Let me know if there is anything else that can be of use.

K7 gravatar imageK7 ( 2015-09-01 06:13:47 -0600 )edit

BTW I'm using ethzasl_ptam. Not sure if it is compatible or not.

K7 gravatar imageK7 ( 2015-09-01 14:51:32 -0600 )edit

@Tom Moore would it help if all the messages had the same time stamp?

K7 gravatar imageK7 ( 2015-09-01 15:29:00 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-09-01 18:58:28 -0600

Tom Moore gravatar image

updated 2015-09-02 21:15:51 -0600

What happens if you turn off linear acceleration? Also, I would turn of roll, pitch, and yaw for pose0.

EDIT:

Run PTAM without anything else running (not even ekf_localization_node). Try running the tf tf_monitor or simply view_frames. Now run your transform launch file and do those actions again. Make sure your frames exist and are connected.

One thing you might want to do regardless is use the tf2_ros static_transform_publisher. The parameters are very similar:

<node pkg="tf2_ros" type="static_transform_publisher" name="odom_cam" args="0 0 0 0 0 0 odom usb_cam" />

Note that you no longer specify the period at the end.

EDIT 2

You have a tf reparenting problem. Something else is publishing a world->usb_cam transform. You are publishing an odom->usb_cam transform. No frame can have two parents (it's a tree structure). You should make your static transform go from odom->world instead of odom->usb_cam.

edit flag offensive delete link more

Comments

I turned off linear acceleration for the imu0 and RPY for pose0. This eliminates the drift but now there is no positional movement at all.

K7 gravatar imageK7 ( 2015-09-01 19:55:52 -0600 )edit

Sounds like PTAM data is being ignored. Try fusing just the PTAM data (comment out the IMU) and see if anything happens. If not, turn on debug mode, run briefly, and search for pose0 in the debug output file.

Tom Moore gravatar imageTom Moore ( 2015-09-01 20:17:06 -0600 )edit

@Tom Moore I've added the debug output to the question at the end (more). I am getting this warning: failed to transform from usb_cam->odom for pose0. Looks like I still haven't got tf setup correctly?

K7 gravatar imageK7 ( 2015-09-01 22:06:02 -0600 )edit

@Tom Moore before running the tf launch file it looks like this: world->usb_cam. After it looks like this: world odom->usb_cam base_link->fcu. If I try it with tf2 nothing appears to change after launching the file: world->usb_cam. I didn't realize ptam was creating that world node before.

K7 gravatar imageK7 ( 2015-09-02 15:52:01 -0600 )edit

That has eliminated (most of) the drift and I think it is now using both sensors. It doesn't seem to be moving right but I will have another look at the tutorial about preparing sensor data and see if I can get that right. Thanks again!

K7 gravatar imageK7 ( 2015-09-02 23:21:15 -0600 )edit

@Tom Moore just to check before moving on, the tf tree that is now generated after running ekf has 2 branches with odom at the root of both: odom->base_link->fcu and odom->world->usb_cam. Does this seem right?

K7 gravatar imageK7 ( 2015-09-02 23:56:59 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2015-09-01 01:56:12 -0600

Seen: 750 times

Last updated: Sep 02 '15