In robot_localization, why does differential mode not use static transforms?

asked 2019-07-23 11:50:17 -0500

Louis Stromeyer
11 ●1 ●1 ●2

Hello, My setup is ROS Kinetic, using the kinetic level of robot_localization on Ubuntu 16.04 I am running into a problem using the differential parameter in the robot localization package. Below is my launch file and a YAML file with the parameters. I am using the differential parameter for my visual odometry which is outputting a PoseWithCovarianceStamped (I know this is unusual output for visual odometry but it is what I have) which I pass as pose0 to the ekf filter I am using (ekf_se_odom). I want the differential data instead so if visual odometry resets it will not start providing data based on an origin centered at its location when it re-initializes.

However, when I turn the differential parameter on, the filter ignores a 180 degree roll I am applying with a static transform publisher (in the launch file below) from my visual odometry sensor. Thus my y-axis values get flipped and yaw changes in the wrong direction. I traced the code that handles this in the robot_localization package. Specifically, ros_filter.cpp starting at line 2542:

// Handle issues where frame_id data is not filled out properly
// @todo: verify that this is necessary still. New IMU handling may
// have rendered this obsolete.std::string finalTargetFrame;
if (targetFrame == "" && msg->header.frame_id == "")
{
  // Blank target and message frames mean we can just
  // use our world_frame
  finalTargetFrame = worldFrameId_;
  poseTmp.frame_id_ = finalTargetFrame;
}
else if (targetFrame == "")
{
  // A blank target frame means we shouldn't bother
  // transforming the data
  finalTargetFrame = msg->header.frame_id;
  poseTmp.frame_id_ = finalTargetFrame;
}
else
{
  // Otherwise, we should use our target frame
  finalTargetFrame = targetFrame;
  poseTmp.frame_id_ = (differential ? finalTargetFrame : msg->header.frame_id);
}

The trinary statement on the last line seems to suggest that when differential mode is on, the source and target frames are both set to targetFrame (which I traced to be the world frame for the filter). So in my case it will cause an odom to odom transformation and thus my static transform is not used.

So my question is why is this the case? Does anybody know the reasoning behind this? Please let me know if any more information is needed from me.

Launch File:

<launch>

  <!-- starting middleman covariance nodes (allows setting of covarainces after data collection) -->
  <rosparam command="load" file="$(find tractor_localization)/params/dual_ekf_navsat_IMU.yaml" />
  <node pkg="pose_covariances" type="pose_covariances.py" name="pose_covariances" clear_params='true' output="screen" />
  <node pkg="imu_covariances" type="imu_covariances.py" name="imu_covariances1" clear_params='true' output="screen" />
  <node pkg="pose_covariances" type="pose_to_odom.py" name="pose_to_odom" clear_params='true' output="screen" />

   <!-- starting static transform publishers -->
  <node pkg="tf" type="static_transform_publisher"
    name="gps_transform"
    args="0.055 -0.095 0 0 0 0 base_link gps 30" /> <!-- should this be to the map frame?? -->

  <node pkg="tf" type="static_transform_publisher"
    name="imu_transform"
    args="-0.068 -0.062 0 -1.763 0 0 base_link base_link_imu1 30" />


  <node pkg="tf" type="static_transform_publisher"
    name="svo_transform_odom"
    args="0.023 0.1 0 0 0 3.1415 odom cam0 30" />

    <!-- x y z yaw pitch roll frame_id ...

(more)

answered 2019-08-23 12:09:10 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2019-08-26 05:09:57 -0500

Let me start with defining a few things.

First, we'll call the pose at time 0 p0, and the pose at time 1 p1. We could just as easily use N and N+1; I'm just trying to refer to consecutive poses from your sensor. The measurements arrive at times t0 and t1, respectively.

Now, let's say you have a system like this:

image description

Here, your odom frame is your EKF world frame, and pose_frame is the pose of your robot in some other world-fixed frame. This could be from, e.g., a motion capture system, or an indoor GPS system. For this example, let's assume it comes from some sensor that is not affixed to the robot, and that measures the robot's full 3D pose. Note that these are both right-handed frames (and therefore compliant to ROS standards); it's just that the odom frame has z pointing "out" of the page, and the pose_frame has z pointing "into" the page.

In differential mode, the EKF computes a delta from two consecutive poses as:

delta1 = p0^-1 * p1

We then convert that to a velocity:

v1 = detla1 / (t1 - t0)

We then feed that data to the filter as a velocity, not as a pose.

Note that as soon as we do that transform, both delta1 and v1 are no longer in the original coordinate frame. They are both actually in the base_link frame of the robot itself. This is correct for velocity data.

EDIT: It's really the case that differentiating two poses will produce a velocity in the sensor frame. It's just that in my example, the sensor in question is measuring the pose of the robot.

Referring back to the figure, we can see that:

In the odom frame, p0 is at position (2, 2, 0) with Euler roll, pitch, yaw values of (0, 0, 0)
In the odom frame, p1 is at position (5, 4, 0) with Euler roll, pitch, yaw values of (0, 0, pi / 2)
In the pose_frame frame, p0 is at position (14, 9, 0) with Euler roll, pitch, yaw values of (pi, 0, -pi/2) * We'll revisit these - this is where the trouble lies
In the pose_frame frame, p1 is at position (12, 6, 0) with Euler roll, pitch, yaw values of (pi, 0, pi)

Here's a code snippet that uses tf2 to compute the deltas:

{
  tf2::Transform p0;
  p0.setOrigin(tf2::Vector3(2.0, 2.0, 0.0));
  p0.setRotation(tf2::Quaternion::getIdentity());

  tf2::Transform p1;
  tf2::Quaternion quat1;
  quat1.setRPY(0.0, 0.0, M_PI / 2.0);
  p1.setOrigin(tf2::Vector3(5.0, 4.0, 0.0));
  p1.setRotation(quat1);

  auto delta1 = p0.inverseTimes(p1);

  double r, p, y;
  tf2::getEulerYPR(delta1.getRotation(), y, p, r);
  std::cout << "Delta is (" << delta1.getOrigin().getX() << ", " << delta1.getOrigin().getY() << ", " << delta1.getOrigin().getZ() << "), (" << r << ", " << p << ", " << y << ")\n";
}

{
  tf2::Transform p0;
  tf2::Quaternion quat0;
  quat0 ...

(more)

edit flag offensive delete link

Comments

Regardless of whether this answers the OP's question: @Tom Moore: this is a very nicely formatted, articulate and very extensive post. I take off my hat to you sir, and give you an imaginary +100 (which unfortunately still only counts as 10 points).

gvdhoorn ( 2019-08-24 06:24:44 -0500 )edit

rmensing ( 2019-08-26 04:30:26 -0500 )edit

@gvdhoorn thanks. It was as much an exercise for me to make sure I wasn't doing something stupid. And now that I've had more time to think about it, I'm not convinced that I wasn't. :)

Tom Moore ( 2019-08-26 05:06:06 -0500 )edit

add a comment

In robot_localization, why does differential mode not use static transforms?

1 Answer

Comments

Question Tools

Stats

Related questions

In robot_localization, why does differential mode not use static transforms? edit

1 Answer

Comments

Question Tools

Stats

Related questions

In robot_localization, why does differential mode not use static transforms?