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:

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:

1. In the odom frame, p0 is at position (2, 2, 0) with Euler roll, pitch, yaw values of (0, 0, 0)
2. In the odom frame, p1 is at position (5, 4, 0) with Euler roll, pitch, yaw values of (0, 0, pi / 2)
3. 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
4. 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.setRPY(M_PI, 0.0, -M_PI / 2.0);  // Note the roll angle of pi
  p0.setOrigin(tf2::Vector3(14.0, 9.0, 0.0));
  p0.setRotation(quat0);

  tf2::Transform p1;
  tf2::Quaternion quat1;
  quat1.setRPY(M_PI, 0.0, M_PI);  // Note the roll angle of pi
  p1.setOrigin(tf2::Vector3(12.0, 6.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";
}

The output is:

Delta is (3, 2, 0), (0, -0, 1.5708)
Delta is (3, 2, 2.44929e-16), (-1.22465e-16, -1.22465e-16, 1.5708)

Those deltas are the same. So as long as the robot's pose is reported consistently in both frames, the differentiation logic will work the same. The actual transform from pose_frame to odom is irrelevant, because the differentiation puts everything in the robot's base_link frame.

The issue in your case is that your sensor's coordinate frame is upside-down, but the poses it's generating for your robot assume the robot is right-side-up within that frame. In other words, if we revise (3) and (4) above, it would be this:

1. In the pose_frame frame, p0 is at position (14, 9, 0) with Euler roll, pitch, yaw values of (0, 0, -pi/2)
2. In the pose_frame frame, p1 is at position (12, 6, 0) with Euler roll, pitch, yaw values of (0, 0, pi)

However, your robot is really upside-down within that frame, so its pose in that frame should have the non-zero roll angles. Contrast this with a 3D motion capture system, which, if your robot was flipped upside-down within it, would detect the roll angle.

In any case, when we compute the new delta as

{
  tf2::Transform p0;
  tf2::Quaternion quat0;
  quat0.setRPY(0.0, 0.0, -M_PI / 2.0);  // Note the roll angle of 0
  p0.setOrigin(tf2::Vector3(14.0, 9.0, 0.0));
  p0.setRotation(quat0);

  tf2::Transform p1;
  tf2::Quaternion quat1;
  quat1.setRPY(0.0, 0.0, M_PI);  // Note the roll angle of 0
  p1.setOrigin(tf2::Vector3(12.0, 6.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";
}

we get

Delta is (3, -2, 0), (0, 0, -1.5708)

which is the issue you're seeing.

While transforming the poses before we differentiate them will still work for the use case I want, I am not yet convinced it would work in the general case for what you want. For example:

{
  // Transforms data to the odom frame from the pose frame
  tf2::Transform pose_frame_to_odom;
  tf2::Quaternion quat_pose_frame_to_odom;
  quat_pose_frame_to_odom.setRPY(M_PI, 0.0, -M_PI / 2.0);
  pose_frame_to_odom.setOrigin(tf2::Vector3(11.0, 16.0, 0.0));
  pose_frame_to_odom.setRotation(quat_pose_frame_to_odom);

  tf2::Transform p0;
  tf2::Quaternion quat0;
  quat0.setRPY(M_PI, 0.0, -M_PI / 2.0);  // Note that we have the roll angle of pi again
  p0.setOrigin(tf2::Vector3(14.0, 9.0, 0.0));
  p0.setRotation(quat0);

  p0 = pose_frame_to_odom * p0;

  double r, p, y;
  tf2::getEulerYPR(p0.getRotation(), r, p, y);
  std::cout << "p0 transformed to odom is " << p0.getOrigin().getX() << ", " << p0.getOrigin().getY() << ", " << y << ", " << p << ", " << r << "\n";

  tf2::Transform p1;
  tf2::Quaternion quat1;
  quat1.setRPY(M_PI, 0.0, M_PI);  // Note that we have the roll angle of pi again
  p1.setOrigin(tf2::Vector3(12.0, 6.0, 0.0));
  p1.setRotation(quat1);

  p1 = pose_frame_to_odom * p1;

  tf2::getEulerYPR(p1.getRotation(), r, p, y);
  std::cout << "p1 transformed to odom is " << p1.getOrigin().getX() << ", " << p1.getOrigin().getY() << ", " << y << ", " << p << ", " << r << "\n";

  auto delta1 = p0.inverseTimes(p1);

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

This yields

p0 transformed to odom is 2, 2, -1.22465e-16, 1.22465e-16, -1.36846e-48
p1 transformed to odom is 5, 4, 1.27036e-33, -1.6268e-32, 1.5708
Delta is 3, 2, -1.22465e-16, -1.22465e-16, 1.5708

...all of which is right. However, if we zero out the roll angles again:

{
  // Transforms data to the odom frame from the pose frame
  tf2::Transform pose_frame_to_odom;
  tf2::Quaternion quat_pose_frame_to_odom;
  quat_pose_frame_to_odom.setRPY(M_PI, 0.0, -M_PI / 2.0);
  pose_frame_to_odom.setOrigin(tf2::Vector3(11.0, 16.0, 0.0));
  pose_frame_to_odom.setRotation(quat_pose_frame_to_odom);

  tf2::Transform p0;
  tf2::Quaternion quat0;
  quat0.setRPY(0.0, 0.0, -M_PI / 2.0);
  p0.setOrigin(tf2::Vector3(14.0, 9.0, 0.0));
  p0.setRotation(quat0);

  p0 = pose_frame_to_odom * p0;

  double r, p, y;
  tf2::getEulerYPR(p0.getRotation(), r, p, y);
  std::cout << "p0 transformed to odom is " << p0.getOrigin().getX() << ", " << p0.getOrigin().getY() << ", " << y << ", " << p << ", " << r << "\n";

  tf2::Transform p1;
  tf2::Quaternion quat1;
  quat1.setRPY(0.0, 0.0, M_PI);
  p1.setOrigin(tf2::Vector3(12.0, 6.0, 0.0));
  p1.setRotation(quat1);

  p1 = pose_frame_to_odom * p1;

  tf2::getEulerYPR(p1.getRotation(), r, p, y);
  std::cout << "p1 transformed to odom is " << p1.getOrigin().getX() << ", " << p1.getOrigin().getY() << ", " << y << ", " << p << ", " << r << "\n";

  auto delta1 = p0.inverseTimes(p1);

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

we get this

p0 transformed to odom is 2, 2, 3.14159, 1.22465e-16, -1.58728e-48
p1 transformed to odom is 5, 4, -3.14159, -2.46519e-32, 1.5708
Delta is 3, -2, 2.54072e-33, -2.56585e-48, -1.5708

I may have made a math error in here somewhere, but I'm not yet convinced that (a) there's an issue with the code, or that (b) changing the line in question will work in the general case. But I can be convinced otherwise. :)

EDIT: I think that, given another recent issue on the GH repo, it makes sense to just transform all pose data into the target frame, and then differentiate it. If we find that fixes your issue, great. If not, we can talk about it more.

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

Asked by gvdhoorn on 2019-08-24 06:24:44 UTC

Related: https://github.com/cra-ros-pkg/robot_localization/issues/482

Asked by rmensing on 2019-08-26 04:30:26 UTC

@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. :)

Asked by Tom Moore on 2019-08-26 05:06:06 UTC