0
$\begingroup$

Rosanswers logo

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:

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

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, 0.001132411300727592, -0.00016460156245969569, -4.971036727512627e-06, 0.251802630760143, -3.866513136687686e-05, -0.001132338668495088, 6.963000174227653e-07, 0.0005564970580662025, -3.8347082308792896e-05, -3.866513136687682e-05, 0.3528520627453843, 0.00015796138409329648, -0.0005195820989177445, 6.036177571201076e-08, 5.617159792226772e-07, -0.0011323386684950888, 0.00015796138409329648, 0.03157641168651724, 7.257794513246155e-10, 4.91036385931138e-07, 0.0011324113007275926, 6.963000174227649e-07, -0.0005195820989177441, 7.257794513253871e-10, 0.03157641321713827, 1.1834706590470503e-06, -0.0001646015624596956, 0.0005564970580662015, 6.036177571201076e-08, 4.910363859311376e-07, 1.1834706590470499e-06, 0.043689017073481766]
twist: 
  twist: 
    linear: 
      x: 0.0210921228234
      y: 0.0062526978065
      z: 0.0547688153658
    angular: 
      x: -0.000258302719997
      y: -0.000517417815119
      z: -0.00076631415027
  covariance: [0.06239698790077553, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06239698790077553, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09983517550002659, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2179398991361593e-07, 5.2601439590013494e-33, 3.0621846696809155e-30, 0.0, 0.0, 0.0, 5.260143958793435e-33, 1.2179398991361593e-07, 8.22777177602546e-30, 0.0, 0.0, 0.0, 3.0621846696809866e-30, 8.22777177602553e-30, 1.218204673952421e-07]
---

Debug output with IMU turned off:

tf_prefix is 
map_frame is map
odom_frame is odom
base_link_frame is base_link
world_frame is odom
transform_time_offset is 0
frequency is 30
sensor_timeout is 0.1
two_d_mode is false
print_diagnostics is false
Subscribed to /vslam/pose (pose0)
    pose0_differential is true
    pose0_rejection_threshold is 1.79769e+308
    pose0_queue_size is 1
    pose0 update vector is [t  t  t  f  f  f  f  f  f  f  f  f  f  f  f  ]
------ RosFilter::integrateMeasurements ------

Integration time is 1441162630.7626991272
0 measurements in queue.
Filter not yet initialized.

----- /RosFilter::integrateMeasurements ------
WARNING: failed to transform from usb_cam->odom for pose0 message received at 1441162630.990499904. No transform exists from source to target frame.
------ RosFilter::integrateMeasurements ------

Originally posted by K7 on ROS Answers with karma: 94 on 2015-09-01

Post score: 0

Original comments

Comment by Tom Moore on 2015-09-01:
Can you show me a sample output message (from the EKF)?

Comment by K7 on 2015-09-01:
Sure. I'll update the question now.

Comment by K7 on 2015-09-01:
@Tom Moore sample output is there now. Let me know if there is anything else that can be of use.

Comment by K7 on 2015-09-01:
BTW I'm using ethzasl_ptam. Not sure if it is compatible or not.

Comment by K7 on 2015-09-01:
@Tom Moore would it help if all the messages had the same time stamp?

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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.

Originally posted by Tom Moore with karma: 13689 on 2015-09-01

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by K7 on 2015-09-01:
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.

Comment by Tom Moore on 2015-09-01:
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.

Comment by K7 on 2015-09-01:
@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?

Comment by K7 on 2015-09-02:
@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.

Comment by K7 on 2015-09-02:
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!

Comment by K7 on 2015-09-02:
@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?

Improve this answer
$\endgroup$

Your Answer

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