Orientation does not seem correct

[chris-ferone]

Hi,

All of the output from the ROS2 driver now looks correct; however, I cannot correctly translate the published orientation reported in the ins/imu and ins/odometry topics to roll, pitch, yaw in a way that makes sense.

The test I conducted was driving the vehicle at a slow speed due south. There was little vehicle roll or pitch. Given that ENU frame is being used, I would therefore expect to see the following:

Roll ~ 0 degrees Pitch ~ 0 degrees Yaw ~ -90 degrees

You can see what the actual reported orientation looked like when I conducted this test in the bottom right section of this image.

As you can see the values are approximately: x = - 0.5 y, z, w = 0.5

If I convert these quaternion values into RPY

tf2::Quaternion qq(0.5, -0.5, -0.5, -0.5);
double roll, pitch, yaw;
tf2::Matrix3x3(qq).getRPY(roll, pitch, yaw);
RCLCPP_INFO(this->get_logger(),"r:%.2f p:%.2f y:%.2f", roll, pitch, yaw);  

the following is printed to the console:

[lv_ground_truth_republisher-1] [INFO] [1641239122.213901124] [lv_ground_truth_republisher]: r:0.00 p:1.57 y:0.00

As you can see, Pitch is equal to 90 degrees, and roll and yaw are zero.

Can you confirm that the orientation contained in the ins/imu and ins/odometry topics is always reported in the ENU frame regardless of how the unit is physically oriented in the vehicle? In other words, a coordinate transformation from the ins frame to base_link should not be necessary for orientation, unlike velocity or acceleration values, which do require a coordinate transformation (when the ins frame and base_link frames are not aligned).

I've also confirmed the reference heading is zero-ed in the ins/nav_sat_ref topic.

[ljones-oxts]

Hi Chris

I'd just like to verify that the way you're constructing the quaternion is correct: according to the documentation (http://docs.ros.org/en/jade/api/tf2/html/classtf2_1_1Quaternion.html#a4c9249826320a3aa30e88a11fc75e577), the constructor looks like:

tf2::Quaternion::Quaternion(const tf2Scalar &x, const tf2Scalar &y, const tf2Scalar &z, const tf2Scalar &w);

Therefore, for your example, I would have expected the appropriate quaternion to be:

tf2::Quaternion qq(-0.5, 0.5, 0.5, 0.5);

Since rotation orders are not likely to be an issue for your simple example, I shall not go into that right now. The only other thing I noticed, upon looking at the documentation and source code for tf2 (as I'm not familiar with it), is that getRPY() accepts an optional fourth argument int solution_number = 1. The relationship between the two solutions, according to the code for getEulerYPR(), is that the pitch for the second solution is equal to Pi minus the pitch for the first solution (and the values for yaw and roll are computed using the pitch, as expected). Thus, if you're not getting a sensible solution when using solution_number = 1, then try with solution_number = 2.

Let me know if you need further assistance.

Thanks

Llyr

`

[ljones-oxts]

Hi Chris

Scratch that last reply - I subsequently looked over your screenshot and realized you are indeed constructing the quaternion correctly!

Let me investigate further.

Thanks

Llyr

[ljones-oxts]

Hi Chris

I've gone through the conversion code (here), and what's going on is:

1. We take the orientation angles in the NED vehicle frame (as reported by NCOM).
2. We build a quaternion from those angles using Quaternion.setRPY()).
3. We convert that quaternion into the ENU vehicle frame.
4. We then convert this to body frame ENU by unrotating the VAT transformation.

So I can at least confirm that we are outputting orientations in the ENU body frame.

From reading the tf2 documentation, I'm not exactly sure that Step 2 above actually does what we think it should do. I'm surprised as we've been using the NCOM driver extensively for development - maybe there are two bugs cancelling each other out in our case but not in yours.

I will investigate further and come back to you.

Thanks

Llyr

[ljones-oxts]

Hi Chris

I'm now relatively sure it's to do with the order in which rotations are applied, and I suspect this happens at multiple steps (including how you get the Euler angles back from the resultant quaternion).

To help me further diagnose, would it be possible for you to give me the following:

1. Orientation angles (roll, pitch, yaw) from the NCOM stream.
2. The VAT (Vehicle-to-IMU) angles as found in the configuration of your INS.

This way, I can construct some unit tests to see where things are going wrong.

Thanks

Llyr

[chris-ferone]

Hi Llyr,

Thanks for the prompt replies. How can I capture the orientation angles from the NCOM stream? How do I access the NCOM stream? Do you simply want the data contained in the ins/ncom topic published by the driver? For question 2, is this what you are asking for?

Thanks, Chris

[ljones-oxts]

Hi Chris

Seeing as you're using a rosbag, perhaps what I'm suggesting won't work (in addition, we're also looking at different reference frames as NAVdisplay displays orientations in the vehicle frame). Thanks for the configuration, that'll hopefully help.

I suggest a trial-and-error method to fixing this, as I unfortunately don't have much test data to hand as I'm currently remoting in from home. I've just pushed two commits to the branch 7-orientations-fix which, at least on the test data I do have, has produced sensible heading/pitch/roll angles in an NED frame. If you could try that out on your data, and see what you get, I think will progress this.

I suspect there is at least one further fix to transform to ENU, but I'd like us to get at least some sensible readings in NED first before trying to solve the next problem.

Thanks

Llyr

[ljones-oxts]

Hi Chris

Just so I can elaborate on what I think is going wrong:

In the OxTS ecosystem, all of our orientations follow the heading-pitch-roll convention, in such a way that the resultant orientation is the result of the following rotations:

q_out = q_z(heading) * q_y(pitch) * q_x(roll)

From comparing the code in tf2 with some reference material on quaternions, I've worked out that tf2's functionality actually performs these rotations in the opposite order, i.e.:

q_out = q_x(roll) * q_y(pitch) * q_z(heading)

Since rotations are not (in general) commutative, these two will yield different quaternions; this, I suspect is the root of our problem. We do also have the additional complexity of everything in the OxTS ecosystem being in a North-East-Down (NED) frame, whereas ROS works in an East-North-Up (ENU) frame. We get around this by multiplying by the following quaternion:

q_enu_ned = q_x(180) * q_y(0) * q_z(90)

This is equivalent to multiplication by the following basis-vector transformation matrix:

M_enu_ned = (0, 1, 0; 1, 0, 0; 0, 0, -1)

The commits I pushed address the first issue by replacing calls to tf2::Quaterion::setRPY() with a helper function which constructs the composition of q_x, q_y and q_z in the correct order. I believe there still remains the issue of decomposing the resultant quaternion back into Euler angles in the correct order, though I could be wrong there.

Regards

Llyr

[chris-ferone]

FYI, this line causes a build error. Should be "/*"

https://github.com/OxfordTechnicalSolutions/oxts_ros2_driver/blob/7363bf8558057540e59a38c969cd483fc7cad2ea/oxts_ins/src/conversions/wrapper.cpp#L21

[chris-ferone]

I built and ran the 7-orientation-fix branch and if I point the vehicle due south, here's the orientation I see in the /ins/imu topic

converting this to quaternion in ROS...

  tf2::Quaternion qq(-0.7, 0.0, 0.7, 0.0);
  double roll, pitch, yaw;
  tf2::Matrix3x3(qq).getRPY(roll, pitch, yaw);
  RCLCPP_INFO(this->get_logger(),"r:%.2f p:%.2f y:%.2f", roll, pitch, yaw); 

...gives me:

[lv_ground_truth_republisher-1] [INFO] [1641331091.475356318] [lv_ground_truth_republisher]: r:3.14 p:1.57 y:0.00

Here's a screenshot of NavDisplay:

I don't know if this is helpful to you, but here's one NCOM message:

`--- header: stamp: sec: 1641331487 nanosec: 170000000 frame_id: oxts_sn40162 raw_packet:

• 231
• 50
• 20
• 32
• 13
• 0
• 135
• 2
• 0
• 16
• 129
• 254
• 143
• 0
• 0
• 214
• 255
• 255
• 67
• 1
• 0
• 4
• 54
• 9
• 41
• 95
• 100
• 26
• 151
• 231
• 63
• 247
• 122
• 37
• 141
• 20
• 85
• 247
• 191
• 18
• 90
• 92
• 67
• 77
• 0
• 0
• 44
• 0
• 0
• 242
• 255
• 255
• 247
• 123
• 208
• 86
• 110
• 0
• 152
• 6
• 0
• 146
• 0
• 229
• 14
• 81
• 1
• 13
• 6
• 6
• 6

• 136

  header:`

[ljones-oxts]

Hi Chris

Thanks for the feedback. I've been thinking it over and I think I'll need to dig around for a test data set that most closely resembles your setup (in particular, your INS is pointing upwards which, from memory, is potentially a gimbal lock so I'm wondering if that's what's significantly different).

I'll inform you when I find something.

Thanks

Llyr

[ljones-oxts]

Hi Chris

I believe I may have misinterpreted your original question:

"Can you confirm that the orientation contained in the ins/imu and ins/odometry topics is always reported in the ENU frame regardless of how the unit is physically oriented in the vehicle?"

The answer here is no - the orientation contained in the ins/imu and ins/odometry topics are always output in the sensor (INS) frame, rather than the vehicle frame, so the orientation of the unit is important. This is achieved by taking the orientation in the vehicle frame as reported by NCOM and applying the inverse of the vehicle to IMU (VAT) transformation (which in your case is heading 0, pitch 90, roll 0). We apply the inverse since we're transforming an orientation (hence it's an extrinsic rather than intrinsic rotation).

However, your example has a particular issue: namely, the VAT transformation induces a gimbal lock configuration (which occurs, for the OxTS system, when pitch is +/-90 degrees). Since the configuration is (HPR) (0, 90, 0), its inverse is naturally (0, -90, 0), which also is a gimbal lock configuration. This means that applying this transformation creates a situation whereby the resultant rotation is not numerically stable (the giveaway in your example is the resultant quaternion having its real component w close to zero). To show this, I've gone through the matrix equations, using your expected (yaw = -90, pitch = 0, roll = 0) as the expected orientation in the vehicle frame (which translates to (heading = 180, pitch = 0, roll = 0):

R_vehicle = R(180, 0, 0) = Matrix(-1, 0, 0; 0, -1, 0; 0, 0, 1);
R_vat_inv = R(0, -90, 0) = Matrix(0, 0, -1; 0, -1, 0; 1, 0, 0);

R_body = R_vat_inv *  R_vehicle = Matrix(0, 0, -1; 0, 1, 0; -1, 0, 0);

Plugging this back into the rotation matrix equation, we get:

-sin(pitch) = -1; => sin(pitch) = 1 => pitch = 90;
cos(heading - roll) = 1;
sin(heading - roll) = 1;

So we get a pitch angle of 90, as you've found. However, this is a gimbal lock scenario, where the heading and roll do not have unique solutions (but are instead related - see my subsequent pose - so there are infinitely many solutions), hence my comment above about being numerically unstable.

I still think we've found an issue with rotation orders, but this does not apply to your example for two reasons:

1. For the vehicle-frame orientation, only the yaw is non-zero hence only one rotation is being applied.
2. For the Vehicle-to-INS transformation, only the pitch is non-zero and it always appears as the second rotation and hence will induce gimbal lock whatever the order (YPR or RPY).

In terms of you getting some angles you can work with: since the above does not have a stable solution, multiplying by the VAT matrix will likely also lead to an unstable result. Hence, I would advise that you remove the conversion in the first place, by editing line 87 of oxts_ins/src/conversions/wrapper.cpp and remove the * vat.inverse() expression, so that it outputs in vehicle frame that is independent of the INS orientation.

This will give you what you need. I'm also fairly sure we'll need a different function to do the angle conversion as that will be in the wrong order again (which doesn't matter in your example, but will matter as soon as you introduce non-zero pitch and/or roll). In the meantime, I will raise this matter internally to see if we can have a better solution (such as adding a configuration option to govern the body/vehicle frame output), and to fix the underlying issues with rotation orders.

[ljones-oxts]

Hi Chris

I've just been examining the rotation matrices and I think everything looks fine except the function which recovers the angles from the quaternion. I'm basing all of this on the master branch, and not on my alterations. Also, remember we apply another transformation to convert from NED to ENU that we must factor in.

The following code uses the rotation matrix to compute the angles (two solutions):

tf2::Quaternion qq(0.5, -0.5, -0.5, -0.5);
tf2::Matrix3x3 mat(qq);
double c20 = mat[2][0];
double pitch1 = -std::asin(c20);
double pitch2 = M_PI + pitch1;
double heading1, heading2;
double roll1, roll2;
if (std::abs(c20) == 1.0)
{
  // Singularity in pitch (pitch = -90)
  roll1 = 0;
  heading1 = -roll1 + std::atan2(-mat[0][1], -mat[0][2]);
  heading2 = 0;
  roll2 = -heading2 + std::atan2(-mat[0][1], -mat[0][2]);
}
else if (std::abs(c20) == -1.0)
{
  // Singularity in pitch (pitch = 90)
  roll1 = 0;
  heading1 = roll1 + std::atan2(mat[0][1], mat[0][2]);
  heading2 = 0;
  roll2 = heading2 - std::atan2(mat[0][1], mat[0][2]);
}
else
{
  double cos_pitch1 = std::cos(pitch1), cos_pitch2 = std::cos(pitch2);
  roll1 = std::atan2(mat[2][1] / cos_pitch1, mat[2][2] / cos_pitch1);
  roll2 = std::atan2(mat[2][1] / cos_pitch2, mat[2][2] / cos_pitch2);
  heading1 = std::atan2(mat[1][0] / cos_pitch1, mat[0][0] / cos_pitch1);
  heading2 = std::atan2(mat[1][0] / cos_pitch2, mat[0][0] / cos_pitch2);
}
std::cout << "Solution 1: roll = " << roll1 << ", pitch = " << pitch1 << ", heading = " << heading1 << std::endl;
std::cout << "Solution 2: roll = " << roll2 << ", pitch = " << pitch2 << ", heading = " << heading2 << std::endl;

I've used your example, and this is what I got:

Solution 1: roll = 0, pitch = 1.5708, heading = 1.5708
Solution 2: roll = 1.5708, pitch = 4.71239, heading = 0

Let's break this down, piece by piece: since we already know the pitch is expected to be +/- 90 degrees, we're in a situation where we have a singularity, so the roll and yaw angles are related. Thus, we must set one of them to zero and compute the other (for completeness, I've put in both solutions).

Going back to our original R_body matrix from the previous post:

R_body = R_vat_inv *  R_vehicle = Matrix(0, 0, -1; 0, 1, 0; -1, 0, 0);

The final step is to convert NED to ENU - while a rotation matrix would suffice, it is more straightforward to model this in terms of a basis vector transformation which sends x -> y, y -> x and z -> -z. So we can very straightforwardly convert R_body into ENU by swapping the top two rows and negating the third:

R_body_ENU = Matrix(0, 1, 0; 0, 0, -1; 1, 0, 0);

Plugging them back into the equations, and following the algorithm above:

If pitch = asin(1) = PI / 2:
   roll1 = 0;
   heading1 = 0 + atan(1 / 0) = PI / 2;
   heading2 = 0;
   roll2 = 0 - atan(1 / 0) = -PI / 2;

If pitch = asin(-1) = - PI / 2:
   roll1 = 0;
   heading1 = 0 - atan(-1 / 0) = -PI / 2;
   heading2 = 0;
   roll2 = 0 + atan(-1 / 0) = PI / 2;

Which matches the results from the quaternion above:

Solution 1: roll = 0, pitch = 1.5708, heading= 1.5708    (AKA Solution 1 when pitch = PI / 2)
Solution 2: roll = 1.5708, pitch = 4.71239 (= -1.5708), heading = 0   (AKA Solution 2 when pitch = -PI / 2)

Hopefully, this all makes sense. Feel free to use the function above in your own code to get sensible angles from your quaternions. Please let me know if you have any further questions.

Thanks

Llyr

[chris-ferone]

Hi Llyr,

I had to put this task aside to complete some other work, but am now ready to return to it.

Thanks for your detailed posts. I appreciate the thorough explanations. Unfortunately, I'm still confused.

The two possible solutions you propose in your previous post seem to be in the FRD/NED frames. Solution 1 only makes sense to me in that context: there is no roll, pitch = 90 solely due to how the sensor is mounted in the vehicle (sensor x-axis pointing down), and heading = 180 in the NED frame because the vehicle is pointing south. What's confusing me here is that you talked about converting from NED to ENU in your previous post, but this solution only makes sense to me in the NED frame. I can't make sense of these values in the ENU frame, so it doesn't seem like that conversion has taken place.

Some additional quesitons:

1. How would one programmatically decide which of the two possible solutions is valid? Is this functionality not something that can be incorporated into the oxts_ros2_driver?
2. Would the problem that I'm facing apply to any RT3000 user trying to use this ROS2 driver who mounts the unit vertically, given that vertically mounting the unit (+/- 90 pitch in FRD INS frame) causes a singularity? Wouldn't this be a problem for anyone using the RT-strut and the ROS2 driver? Why is this not an issue with the ros1 driver for the RT3000? https://bitbucket.org/DataspeedInc/oxford_gps_eth/src/master/

Thanks, Chris

[ljones-oxts]

Hi Chris

Thanks for your reply. Just wanted to let you know I've read your reply and am working on addressing your questions. I will reply with more detail once I've had a chance to work through it.

Thanks

Llyr

[ljones-oxts]

Hi Chris

Ok, I've gone through the example again, with a couple of corrections to misconceptions I have since been corrected on - for example, there is no concept of intrinsic vs. extrinsic rotations here, so we do not have to apply inverses.

I also believe I got the Vehicle-to-IMU matrix incorrect - it should correspond to a rotation of 90 degrees in pitch:

R_veh_body = (0, 0, 1; 0, 1, 0; -1, 0 ,0)

The orientation matrix of the vehicle, expressed in the vehicle frame (FRD), is:

R_orient_veh = (-1, 0, 0; 0, -1, 0; 0, 0, 1)

This can be seen as the rotation matrix where a heading of 180 degrees is applied (cos PI = -1; sin PI = 0).

Transforming this matrix into the body frame, we get:

R_orient_body = R_veh_body * R_orient_veh = (0, 0, 1; 0, -1, 0; 1, 0, 0)

Applying the angle-recovery algorithm above, we find that:

pitch_1 = -asin(1) = -PI/2
roll_1 = 0
heading_1 = 0 + atan2(0, -1) = PI

This leads to the second misconception that I had - these angles correspond to the transformation from the local navigation (NED) frame to the body/IMU frame (FRD). Let's use your "due south" example - so we would expect a unit-scale velocity, in the local navigation frame, to be: v_ned = (-1, 0, 0). Luckily, R_orient_veh is its own inverse, so we can just pre-multiply:

v_veh = R_orient_veh.inverse() * v_ned = (-1, 0, 0,; 0, -1, 0; 0, 0, 1) * (-1, 0, 0) = (1, 0, 0)`

This makes sense, as it is a purely forward velocity in the vehicle frame. In the body/IMU frame, we have:

v_body = R_veh_body * v_veh = (0, 0, 1; 0, 1, 0; -1, 0 ,0) * (1, 0 ,0) = (0, 0, -1)

So as far as the IMU is concerned, this is a downwards motion, which again makes sense in relation to the configuration.

So far, we have only dealt in the NED frame. In the ENU frame, we apply the change-of-basis NED -> EN(-D):

R_orient_body_enu = R_ned_enu * R_orient_body = (0, 1, 0; 1, 0, 0; 0, 0, -1) * (0, 0, 1; 0, -1, 0; 1, 0, 0) = (0, -1, 0; 0, 0, 1; -1, 0, 0)

Again, applying the angle recovery formulae:

pitch_1 = -asin(-1) = PI / 2
roll_1 = 0
yaw_1 = 0 + atan2(1, 0) = PI / 2

To convince ourselves that this indeed does make sense, we again use our "pure south" example:

v_veh_enu = R_ned_enu * v_enu = (0, 1, 0; 1, 0, 0; 0, 0, -1) * (0, 1, 0) =(1, 0, 0)`

In an FLU vehicle frame, this again makes sense.

This covers the rotation matrices. I've had a look at the quaternions and they also check out:

qq = (0.5, -0.5, -0.5, -0.5)
qq_matrix = (0, -1, 0; 0, 0, 1; -1, 0, 0) <-- same as R_orient_body_enu above

Reversing the ENU<->NED transformation, we get:

qq_ned = qq * R(180, 0, 90) qq_ned_matrix = (0, 0, 1; 0, -1, 0; 1, 0, 0) <-- same as R_orient_body above

Thus, when we compare the angles of R_orient_body_enu above to the solution obtained earlier:

Solution 1: roll = 0, pitch = 1.5708, heading= 1.5708 (AKA Solution 1 when pitch = PI / 2)

In the body/IMU frame, these do indeed make sense. To get sensible angles in vehicle frame, what you actually need to do is three-fold:

1. Convert this quaternion into NED.
2. Multiply the NED quaternion by the inverse of the vehicle-to-body transformation.
3. Convert NED to ENU, treating NED as equivalent to FLU (see example).
4. Recover angles of this resultant quaternion.

To illustrate, we start with the qq_ned_matrix above:

qq_matrix_veh = R_vehicle_body.inverse() * qq_ned_matrix
qq_matrix_veh = (0, 0, -1; 0, 1, 0; 1, 0 0) * (0, 0, 1; 0, -1, 0; 1, 0, 0) = (-1, 0, 0; 0, -1, 0; 0, 0, 1)

So far, so good: this maps Forward to South, Right to West and Down to Down. Recovering angles of this matrix gives:

pitch_1 = -asin(0) = 0
roll_1 = 0
heading_1 = 0 + atan2(0, -1) = PI

Which is exactly what we would expect, given our discussion above, in the NED frame.

In the ENU frame, the only way I've been able to get this to work properly is by treating the NED frame as being synonymous with the (body) FLU frame, and then simply doing a conversion from FLU to ENU:

qq = (0.5, -0.5, -0.5, -0.5)
qq_matrix = (0, -1, 0; 0, 0, 1; -1, 0, 0)
qq_ned = qq * R(180, 0, 90) = (0, 0, 1; 0, -1, 0; 1, 0, 0)
R_body_veh = R_veh_body.inverse() = (0, 0, -1; 0, 1, 0; 1, 0, 0)
R_ned_veh = R_body_veh * qq_ned = (-1, 0, 0; 0, -1, 0; 0, 0, 1)
// The trick is to left-multiply R_ned_veh by a matrix as follows:
R_enu_veh = (0, 1, 0; 1, 0, 0; 0, 0, 1) * R_ned_veh = (0, -1, 0; -1, 0, 0; 0, 0, 1);

Recovering angles of R_enu_veh:

pitch = -asin(0) = 0
roll = atan(0 / 1, 1 / 1) = 0
yaw = atan(-1 / 1, 0 / 1) = - PI / 2

Which gives us the angles we would expect from a due south orientation. I think the reason why this trick works is that the matrix I'm using to left-multiply above is the change-of-basis matrix to go from a FRU frame to an ENU frame.

I have spent hours deriving the calculations, and trying different ways of going from NED/ENU and this is the only one which gives us an answer that makes sense.

Thanks

Llyr

[RonaldEnsing]

I suspect that even the example is incorrect.

I'm on version 25d4e005f3b3c985d00f4f417a7c0ea27cb6f714 and running:

ros2 launch oxts visualise.py ncom:=tests/test.ncom

The /ins/odometry seems incorrect. The arrows of the rviz odometry display type (http://wiki.ros.org/rviz/DisplayTypes/Odometry) are not pointing in the direction of movement. This is visible after selecting 'arrows' instead of 'axes' in rviz for the shape of the odometry display type.

[RonaldEnsing]

I'm suspecting that there is no issue with the orientation after all. It's possibly just a visualization issue.

The odometry visualized in rviz is based on the pose (https://docs.ros2.org/foxy/api/nav_msgs/msg/Odometry.html), not the direction of movement. The arrow would point forward in the driving direction of the robot if the robot's base_link conforms to REP105 (https://www.ros.org/reps/rep-0105.html).