Use robot_localization to integrate fixed overhead camera with wheel odometry

asked 2015-08-23 16:19:41 -0600

LazyMonkey
1 ●1 ●2 ●2

updated 2015-09-08 02:48:27 -0600

Hello, I'm trying to integrate the wheel odometry's measurements of a Sphero robot with the position measurements obtained from a visual tracker algorithm (TLD) running on the stream of images of an overhead camera (Kinect v2).

Since the visual tracker algorithm is giving me the 2D position of the robot, I was thinking to use the 'PoseStamped' input message type.

The sphero's odom message contains position and twist (orientation is always the identity quaternion); I'm using the sphero ros package.

I'm trying to use the robot localization package, but I'm a little confused and I have some questions:

What does 'absolute' measurement mean? It is relative to the 'world frame'?
The visual tracker sometimes tracks false positives in other points of the image, and sometimes loses completely the tracking, so it's right to say is subject to discrete jumps. The position obtained from the visual tracker is in a frame called 'world frame', and the position obtained from the odometry it's in another transform tree, in the odometry frame. I need the transform from the world frame to the odom frame, so, it is right to set the map frame and the world frame to the same value (world frame)? Do I need to start more than one instance of the ekf localization node?
In the end, what's the best strategy to use in my case for the sensor fusion?

This is my 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="true"/>

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

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

  <param name="odom0" value="odom"/>
  <param name="pose0" value="robot_2d_geometry_pose"/>

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

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

  <param name="odom0_differential" value="false"/>
  <param name="pose0_differential" value="false"/>

  <param name="odom0_relative" value="false"/>
  <param name="pose0_relative" value="false"/>

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

  <param name="odom0_queue_size" value="4"/>
  <param name="pose0_queue_size" value="5"/> <!-- default 10 -->

  <param name="odom0_pose_rejection_threshold" value="5"/>
  <param name="odom0_twist_rejection_threshold" value="1"/>
  <param name="pose0_rejection_threshold" value="2"/>
  <param name="twist0_rejection_threshold" value="1.2"/>
  <param name="imu0_pose_rejection_threshold" value="0.3"/>
  <param name="imu0_twist_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>

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

  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <rosparam param="process_noise_covariance">[0.5,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.5,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0 ...

(more)

answered 2015-08-29 09:39:37 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2015-09-10 19:49:00 -0600

Yes, an "absolute" measurement means a measurement that is relative to the world (typically map or odom, but can be anything) frame.
For your setup, I would suggest the following:
- You need something to generate the odom->base_link transform. For some robots, the node that generates the wheel encoder-based odometry message can also broadcast the odom->base_link transform. If yours does that, I would suggest turning that on (it may be on by default). If you were to add an IMU, I would stop your robot's driver from generating the odom->base_link transform and add another ekf_localization_node instance that fuses just the odometry and IMU data, and has the world_frame parameter set to odom.
- Assuming that you have something generating the the odom->base_link transform, then I think your configuration above is pretty good. As it is, it will generate that world->odom transform. I would ditch all the _rejection_threshold settings except for pose0_rejection_threshold. You can use that value to reject your noisy measurements from your visual tracker.

EDIT

Part of the trouble you're having is that your /odom topic is reported in the odom frame, but the filter needs to transform it to the map frame before fusing it. Since the filter itself is responsible for the map->odom transform and the headings between the two frames disagree, the transform heading keeps shifting, and you get the result you see.

This can be fixed, however. Does your /odom topic report linear and rotational velocity? If so, you should set X velocity, Y velocity, and yaw velocity for that topic to true, rather than X, Y, and yaw. Your bag file doesn't contain any motion, so it's hard to tell whether those are reported for your robot. You can leave your pose0 configuration in your latest bag file alone. I filtered your bag file to remove the transforms created by the EKF, edited your launch file, and ran everything, and it worked for me. Here's the launch file (eliminating covariance matrices for the sake of brevity):

<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="true"/>

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

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

    <param name="odom0" value="odom"/>
    <param name="pose0" value="robot_2d_geometry_pose"/>

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

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

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

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

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

    <param name="odom0_queue_size" value="4"/>

    <param name="pose0_rejection_threshold" value="2"/>

    <param name="debug"           value="false"/>
    <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  </node>

</launch>

One thing I should change about the EKF is that it should let you turn on ... (more)

edit flag offensive delete link

Comments

Thanks for the answer!

Before reading your answer I've tried to make it work just only Sphero's odometry, without the Visual Tracker, and it worked! (Even if it was a little slow on the converge).

LazyMonkey ( 2015-08-31 11:52:16 -0600 )edit

Here a screenshot, the red arrow is the robot odometry and the yellow arrow is the filtered odometry: - https://www.dropbox.com/s/to00ub5dvrd...

LazyMonkey ( 2015-08-31 11:52:25 -0600 )edit

Then I've tried your solution, first with the pose0 differential parameter set, then unset, but the Sphero odometry and the filtered odometry do not match, also, the filtered odometry contains high values of angular velocity even if the robot is still

LazyMonkey ( 2015-08-31 11:53:14 -0600 )edit

This is seen on rviz as the filtered odometry arrow keeps on spinning and never converge: - https://www.dropbox.com/s/dw4eqdtnvu1... - https://www.dropbox.com/s/82jyxz1uh6h...

LazyMonkey ( 2015-08-31 11:53:59 -0600 )edit

This is a link to a screenshot of the visual tracker running: - https://www.dropbox.com/s/f1wvtr5gr5o...

LazyMonkey ( 2015-08-31 11:54:14 -0600 )edit

This is a link to a 15 seconds bag file with the odometry topics: - https://www.dropbox.com/s/fyfh6do6zir...

LazyMonkey ( 2015-08-31 11:54:42 -0600 )edit

This is my (slightly) updated launch file: - https://www.dropbox.com/s/0b1uiu2kdm4...

LazyMonkey ( 2015-08-31 11:56:06 -0600 )edit

I just can't understand why it's not working ...

LazyMonkey ( 2015-08-31 11:57:18 -0600 )edit

see more comments