covariance ellipse for position does not seem to be drawn correctly

[protobits]

I've played with some values and it seems that for certain combinations of values, the ellipse seems to be wrongly oriented.

[NikolausDemmel]

I also noted wrong orientations, in particular the swapping logic in case of repeated eigenvalues seems a bit fishy (

rviz_plugin_covariance/src/covariance_visual.cpp

Lines 55 to 78 in 7659f4a

	if(Ogre::Quaternion(rotation).Norm() != 1.0f)
	{
	ROS_WARN("computeRotation found non-unitary quaternion!");
	// Check for swapped eigenvectors within repeated eigenvalues
	if(pair.second(0) == pair.second(1))
	{
	ROS_WARN("repeated eigen values 0 and 1... attempting swap");
	for (unsigned i = 0; i < 3; ++i)
	{
	rotation[i][0] = pair.first(i, 1);
	rotation[i][1] = pair.first(i, 0);
	}
	}
	else if(pair.second(1) == pair.second(2))
	{
	ROS_WARN("repeated eigen values 1 and 2... attempting swap");
	for (unsigned i = 0; i < 3; ++i)
	{
	rotation[i][1] = pair.first(i, 2);
	rotation[i][2] = pair.first(i, 1);
	}
	}
	}

). At the very least in the if statements the comparisons should not be == but rather compare with some epsilon.

I frequently observe the convariance of a 2D position (where the ellipsiod is close to a flat disk) flip back and forth, between a correct orientation and one that is rotated upright by 90 degrees.

Does someone know a good reference where it is described how the ellipsoid should be computed properly and how to address numerical instability and repeated eigenvalues?

[NikolausDemmel]

@georgebrindeiro, you added the swapping initially, maybe you can comment?

[protobits]

A quick look in Google throws many possible solutions for going from 3D
covariance to ellipsoid. I suspect the problem is the reordering of
eigenvectors, but I'm not quite sure.

On Wed, Jan 27, 2016 at 3:28 PM, Nikolaus Demmel notifications@github.com
wrote:

@georgebrindeiro https://github.com/georgebrindeiro, you added the
swapping initially, maybe you can comment?

—
Reply to this email directly or view it on GitHub
#10 (comment)
.

[NikolausDemmel]

I tried using http://wiki.ros.org/ecl_statistics/Tutorials/Covariance%20Ellipsoids and this seems to work. I try to get a proper patch out soon.

[NikolausDemmel]

(here is the quick hack for reference: zeno-way@ba75434)

[NikolausDemmel]

(The orientation of the covariance seems to still be not quite right for me, but at least I don't get the flipping disc)

[georgebrindeiro]

Hi, sorry for the delayed response. It's been a while since I worked with this, but I feel this quote from your ecl_statistics reference is crucial:

Note that the axes are sorted (to ensure a right-handed co-ordinate system) and normalised.

The thing is ellipsoid orientation is dependent on eigenvector decomposition, but axes can get jumbled up every time you update. If I remember correctly, eigenvectors are sorted by decreasing eigenvalue.

A non-unitary quaternion (i.e. something that doesn't properly represent orientation) is a good hint something got messed up, usually because you tried to convert a non-right-handed set of vectors into a quaternion, so my hack was to recalculate by swapping axes of same length.

Honestly, I don't remember if I found some theoretical foundation to what I was doing back then or if it just seemed right for the subset of cases I tested.

If there is some canonical method to order eigenvectors so they form a right-handed coordinate system, I think that's the real solution to the problem. I'm not aware of such a method, but I can't say I drilled down this hole for long.

[Ellon]

Hi all,

I also think there's another problem with the covariance drawing. For example, this test script sends odometry messages from a frame_id "base_link" to a child_frame_id "odom_msg", increasing and decreasing periodically the covariance on x. Since the PoseWithCovariance in the Odometry msg is defined on the frame "base_link" I think the covariance should increase/shrink in the x direction of "base_link", but what I see here is that it's changing on the x-axis of the "odom_msg" frame.

Is that like it's supposed to be?

EDIT: I forgot to tell that this file is in a branch I made some improvements on Pose and Odometry displays with covariance, like described in #11 .

[protobits]

@georgebrindeiro from what you quoted, and given that @NikolausDemmel has used that code which supposedly returns the axis in correct order, wouldn't that be the way to fix this?

[georgebrindeiro]

I guess so, but we can only be sure through testing.

[Ellon]

From some tests I did I saw that the matrix returned from ecl_statistics's axes() is just the column matrix of eigenvectors sorted by the decreasing order of their respective eigenvalues, which is what was done before. Eigen gives the eigenvalues and eigenvectors ordered, so this can be done without adding the dependency of ecl to this package, and without swapping columns of the matrix.

[protobits]

The point is not so much the ordering itself (that in itself should not
cause problems, since eigenvalues follow eigenvectors) but if this order
produces a right-hand coordinate system. According to ecl this is ensured.
If not using ecl, this should be done as well.

On Wed, Feb 24, 2016 at 9:54 AM, Ellon Paiva Mendes <
notifications@github.com> wrote:

From some tests I did I saw that the matrix returned from ecl_statistics's
axes() is just the column matrix of eigenvectors sorted by the decreasing
order of their respective eigenvalues, which is what was done before. Eigen
gives the eigenvalues and eigenvectors ordered, so this can be done without
adding the dependency of ecl to this package, and without swapping columns
of the matrix.

—
Reply to this email directly or view it on GitHub
#10 (comment)
.

[georgebrindeiro]

I agree Eigen should be used if ecl_statistics doesn't add any provisions that would help with the issue at hand, but I think that ordering eigenvectors in decreasing order of eigenvalues doesn't guarantee a right-handed coordinate system. It just lists axes from greatest to least position uncertainty.

[georgebrindeiro]

If someone is willing add the code to reorder axes, I found it here in ecl_statistics:
https://github.com/stonier/ecl_core/blob/indigo-devel/ecl_statistics/src/lib/covariance_ellipsoid.cpp#L250

I agree with @Ellon that there is no need for the dependency in ecl_statistics just for that, but I currently can't commit to contribute.

[Ellon]

@v01d Ok! Now I understood the problem.

@georgebrindeiro Thanks for the link. I added this to the code on #11

[protobits]

@Ellon I've compiled the code from your PR and run the test script. It looks like this, which seems correct AFAIK:

[protobits]

By the way, this assumes that the covariance matrix is w.r.t. to the global frame, not the local. Since If I set the orientation to turn 90 degress in yaw, the covariance still grows in the same global X axis.

I couldn't find whether the covariance is assumed to be in the local or global frame. Anyone knows what should be the correct interpretation?

[Ellon]

@v01d Thanks for testing the code. In fact, the position covariance is being drawn wrt. the frame_id stored in the message, in this case "base_link". If you change the fixed frame to other frame you'll see the covariance turning. Try to launch both test scripts currently on the branch at the same time and you'll have more frames on TF to fix on rviz to test.

I decided to do like this because it's like I see the covariance of a pose should be interpreted. If a pose is attached to a frame_id, its position covariance should be expressed on this frame. On the other hand, the orientation covariance should be expressed in the local pose frame. This is currently how it's being done, although the cone shape not at all intuitive to understand the orientation covariance. I have an idea of how to show this properly, taken from the figure 2 of this article that I found on this email exchange on ros-users, but I don't think I'll be able to implement it until next week.

[protobits]

I understand the rationale, I was just wondering if there's a standard behind this, since for many aspects in ROS there already are. I agree that in the absence of more information it sounds as if it would be the expected interpretation. For example, linear velocity is also w.r.t. frame_id while angular velocity is w.r.t. the own's frame.

@georgebrindeiro if you agree on the PR #11 I believe the issue could be considered fixed.

[georgebrindeiro]

@v01d Looks good to me

[Ellon]

@v01d I think that what we can do is interpret the covariance of a message, and not all the covariances in general. For instance, the Odometry msg explicit that the PoseWithCovariance

should be specified in the coordinate frame given by header.frame_id

while the velocities in the twist

should be specified in the coordinate frame given by the child_frame_id

So, for the Odometry message I think it's twist covariance should be expressed in the local frame, or maybe put the linear part in the local frame and the angular in another virtual frame attached to the local frame the but I don't see how yet. I need to think more about it.

[Ellon]

To help understand what I'm saying here I updated the test scripts to show my rationale. The script send_covariance_msgs.py sends a static PoseWithCovariance msg with a larger covariance on x, and a normal Pose msg where it's x value changes +- the standard deviation. We see that on rviz the moving pose moves exactly along the length of the ellipse when the scale is set to 1.

[Ellon]

@v01d @georgebrindeiro I'm closing the Issue since you both agreed on it.

[protobits]

Perfect!
El mar 2, 2016 9:18 AM, "Ellon Paiva Mendes" notifications@github.com
escribió:

@v01d https://github.com/v01d @georgebrindeiro
https://github.com/georgebrindeiro I'm closing the Issue since you both
agreed on it.

—
Reply to this email directly or view it on GitHub
#10 (comment)
.

[NikolausDemmel]

I have a few comments adding to the discussion above. I will add them here even if the issue is closed, since they directly relate to the discussion. Sorry for being late to the party. Let me know if I should open a new issue instead.

First of all, thanks @Ellon. It seems that your merged PR #11 has fixed my display issues. The flickering is gone and the orientation of the positional covariance ellipse seems to be correct as well (i.e. correctly respecting the frame_id as the frame in which the covariance is expressed, unlike before).

I am displaying pure 2D poses, i.e I have all 0 in the covariance rows for z, roll, pitch. The display of the positional covariance is as expected (a flat ellipse). I don't see any cone for the orientational covariance, which is probably due to the 0 variance in roll and pitch, but I don't really know how that should look anyway. What I could imagine is changing the display of the orientation covariance to something that is similar to the linked-to paper above. I.e. one cone (or maybe just the 2d ellipse as in the paper) for each axis. For my 2D case that would degenerate to just two flat same-sized triangles along the x and y axes, where the length of the line that is perpenticular to the axis is 2sigma of the yaw variance ("radius" = 1 sigma). The cone along the z axis would not be visible (width 0). For me that would be nice to have, but it is not really vital currently.

One more thing about the reference frames, just to be clear that we are on the same page. We do agree, that in a PoseWithCovarainceStamped, as well as the PoseWithCovariance inside an Odometry message, the covariance (both positional and orientational) should be expressed in the frame specified in header.frame_id (lets call this the "fixed-frame") and displayed accordingly, right? If we call the frame whose pose is expressed by the PoseWithCovariance message "local-frame", i.e. the pose expresses the transformation from local-frame into fixed-frame, then the positional covariance ellipse needs to be positioned on the local-frame, but oriented in the fixed-frame. My tests suggest that for the positional covariance this is already done correctly. For the rotational covariance this means that for realizing a display such as the one suggest above with three cones along the axes of the local frame, one would first need to convert the positional covaraince from fixed-frame into the local frame and then render the cones in local-frame along the x, y, and z axes. Does that make sense?

[Ellon]

Hi @NikolausDemmel ! Thanks for testing the PR. I'm glad it solved some of your problems.

It's true that I'm only thinking on 3D poses because I'm working with them, but to have a complete plugin it should handle 2D poses as well. Thanks for reminding me this. I'll open some issues now so we can keep track of them.

I think I read somewhere that when representing a pose 2D in a PoseWithCovariance we should set the covariance of z, roll and pitch to -1, but I don't remember where. Anyway, I think this should be a good standard to define 2D pose. Adding a test to check if this condition holds should be enough to trigger the drawing 2D or 3D. The drawing of 2D covariance is in fact easier than in 3D in both positional and rotational cases. I just wonder how the covariance shapes would behave with a scale set to zero: I imagine that the shape would be flat, but it may in fact disappear from the screen. This should be tested properly.

I think we're in the same page regarding the frames. I just like to call child_frame_id the frame the pose is defining (this is in fact a field on Odometry messages, but it's implicit in the PoseWithCovariance messages). So the positional covariance should be expressed on header.frame_id (this is what it's being done) and the rotational on child_frame_id( what is also being done, although the drawing logic itself is not proper for displaying the rotational covariance). I'm afraid the PR I merged screwed your orientation display for 2D cases. I'll provide a fix soon.

[NikolausDemmel]

I think I read somewhere that when representing a pose 2D in a PoseWithCovariance we should set the covariance of z, roll and pitch to -1, but I don't remember where.

The only thing I know of regarding -1 in covariances is the convention in the Imu / Odometry message, that a -1 in the first element of a covariance indicates that the value is not reported, whereas a covariance of all 0 means, that the covariance is not reported (but the "mean" value is). E.g. for the IMU message, if your IMU does not compute orientation, you should indicate that with a -1 in the first element of the orientation covariance. If it computes orientation, but no covariance for it, put all 0.

I don't know if this as also a thing for indicating that certain dimensions of a 3D pose (e.g. z, roll, pitch) are to be ignored, but I would actually prefer to simply handle 0s in certain dimensions of the covariance properly. I.e. if the covariance of z is all zero, the covariance is a flat disk. This actually already works well with the current master, i.e. the sphere with height 0 is not "invisible", but looks like a flat disk as it should. I.e. I propose that the same rendering procedure is used for full 3D poses and poses with 0 variance in some dimensions. Notice that this way the fact that a pose has only a 2D covariance is not indicated by the 0 on the diagonal, but by a 0 eigenvalue. That way the display should work even if the "2D" pose is rotated such that a tilted covariance disk is what we want (for such a covariance there are no 0 elements on the diagonal AFAIK).

I think we're in the same page regarding the frames. I just like to call child_frame_id the frame the pose is defining (this is in fact a field on Odometry messages, but it's implicit in the PoseWithCovariance messages).

Agreed on that naming.

So the positional covariance should be expressed on header.frame_id (this is what it's being done) and the rotational on child_frame_id (what is also being done, although the drawing logic itself is not proper for displaying the rotational covariance).

So the orientation covariance would need to be transferred to child_frame_id, but I'm not sure this is simply a matter of using the rotation matrix, since the orientation covariance refers to euler angles (static axes, x,y,z order). I would have to think about this more.

I'm afraid the PR I merged screwed your orientation display for 2D cases. I'll provide a fix soon.

Oh sorry I made that impression. The display works great for position. The orientation cone does not show up since it probably is 0 width/height in some dimension, but I wouldn't know how it should look anyway and I wasn't using the orientation part before anyway. We can discuss how it should look in the issue you opened.

[Ellon]

The only thing I know of regarding -1 in covariances is the convention in the Imu / Odometry message

Yes, you're right! That's where I saw it. OK, I see I mixed up things. It seems you're right about the zero eigenvalue thing AFAIK but I prefer checking if the components z, roll and pitch of the covariance's diagonal are <= 0 to trigger the 2D visualization. I think this way the code will be simpler to maintain.

So the orientation covariance would need to be transferred to child_frame_id

That's exactly what's being currently done, or at least I think so: positional covariance follows orientation of header.frame_id and the rotational covariance follows the orientation defined by the pose composed with header.frame_id (what we named child_frame_id). You can try it publishing a PoseWithCovariance whose position is fixed and whose orientation turns around one of the axis: you should see the cone following the orientation of the pose.

We can discuss how it should look in the issue you opened.

Yes, let's discuss on the other issues. :)