Normalize quaternions from msgs before Ogre use #1179
Conversation
|
I just ran into this issue today, spent ~4 hours trying to understand why some machines it was working until I upgraded them all. Thanks for this patch @rhaschke! |
There was a problem hiding this comment.
I believe it would it be cleaner if you reverted #1167 then only had your proposed changes here
| Ogre::Quaternion ori(pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z); | ||
| Ogre::Quaternion ori; | ||
| if (!normalizeQuaternion(pose.pose.orientation, ori)) | ||
| ROS_WARN("invalid quaternion (zero length)"); |
There was a problem hiding this comment.
nit: capitalize Invalid here and in rest of this PR
src/rviz/validate_quaternions.h
Outdated
| { | ||
| return validateQuaternions( msg.orientation ); | ||
| float norm2 = quaternionNorm2(w, x, y, z); | ||
| if (norm2 < 10e-3f) |
There was a problem hiding this comment.
can you add a comment here why this threshold was chosen?
src/rviz/validate_quaternions.h
Outdated
| return validateQuaternion(msg.pose.orientation); | ||
| } | ||
|
|
||
|
|
|
I've tested this patch locally and it fixes the issue for me, thanks again |
|
I'm not sure that I agree with the logic to just "normalize" the quaternion on input. Sending an unnormalized quaternion is like sending a NaN for a float value. Just pushing it to the nearest valid float real value is not representing the same thing. When doing quaternion math floating point arithmetic errors can induce quaternions that are non-normalized and they do periodically need to be renormalized. Renormalizing at every step is a very high overhead, normalizing never will end up with NaN values. There is a balance in between to two extremes. This is typically done in a specific time in processing. Often I would expect it to be done shortly before being output from an algorithm not on the input. As the input will not have semantic knowledge of the previous potential cause of non-normalization. And I think it's much more correct for input validation instead of input normalization. Related to this arbitrarily normalizing all inputs can cause completely invalid quaternions (such as unset) to become valid and be rendered. Normalization can only be considered valid if the original is close to normalized, or in a known state of non-normalization. (for example unitialized with all zeros) I can certainly take 4 numbers and turn them into a normalized quaternion, but if they still represent the same underlying information is not clear. Thus it would be better to reject the invalid data rather than pushing it into the right form. Another example of this might be if the throttle commands for a vehicle are on the range 0 to 1. If I receive a command of 1.1 what does that mean? It's undefined as it's not in the interval, it could be someone trying to dial it up to 11. Or it could be someone sending 1.1 out of a range on 100. The meaning of a non-normalized quaternions is similarly undefined. I would strongly advocate for resolving this issue by making sure that all published quaternions are valid rather than accepting invalid quaternions by auto-renormalizing them. |
|
@tfoote To some extend I can agree to your argumentation. However, it's a plain fact, that rviz did (and still does if allowed to) normalize quaternions incoming via ROS msgs and now it simply rejects them. This dramatically changes behaviour of rviz and - besides others - breaks the plain interactive marker tutorial (which could be thought of a best-practice example). |
|
We're looking at restoring the permissive behavior to restore existing behavior, but adding warnings. Anywhere that is known to be sending unnormalized quaternions should be ticketed for resolving at the source. Interactive markers tutorials or libraries should not be sending unnormalized quaternions. Please open tickets for anything that's having trouble right now while we're working to resolve this. |
Is that true? Looking at #1167, I don't see where code that did normalize incoming marker message quaternions was removed. Perhaps you mean in other places?
I can't speak for @tfoote, but we're on the same page here, the disruptive change in behavior was unexpected and we're going to revert it and do something about it. I think the idea of requiring valid quaternions would be a future change or at least would be opt-in if back ported. I think it's worth pursuing to get valid quaternions in rviz (either fixed on the rviz side or on the publishing side), because there have been other issues which attribute crashes to invalid quaternions, but we haven't conclusively linked those two things yet, e.g.: |
|
I don't believe we ever normalised un-normalised quaternions before 1167, but @rhaschke pointed out that, previously, null quaternions would get set to identity in |
|
The unexpected breakage of RViz is not nice, but patching up invalid data just so it can be rendered makes no sense to me, so my vote would be for the warn/error-and-discard option. That would be similar to TF and other incoming msgs that can't be transformed properly: they're discarded and a warning/error with sufficient visibility is output (ie: "Dropped N% of msgs so far, .."). Normalising unconditionally will probably just mean that other nodes will crash/error out on the same data, which could be equally confusing ("but RViz does accept my quaternion?"). |
|
To clarify my point:
So, its not only I fully understand the wish to handle normalization once at generation time, but current practice is fundamentally different. I guess, if you enable a warning in |
This reverts commit b329145.
Normalization of quaternions usually is done by rviz::FrameManager::transform() which transforms a ROS pose into an Ogre position + orientation. Only in some rare cases, a ROS quaternion was directly used as an Ogre::Quaternion, which then requires handling of null quaternions (as they arise from uninitialized ROS pose msgs).
1d30875 to
76e3a52
Compare
|
I addressed @davetcoleman's comments. |
... as suggested in ros-visualization#1182
|
I agree we should require all input to be normalized in the future but for now it should just produce warnings instead of breaking Rviz worldwide. I have found regressions in several different uses of Rviz, including MoveIt!. |
|
@rhaschke thank you for clarifying that tf will normalise the quaternions inside Since we still want warnings for unnormalised quaternions I'll remove the reverting of 1167 from this PR in favour of #1182, and this PR will just focus on normalising the quaternions from ROS msgs that are not otherwise normalised. Additionally I'll invert the logic to consider null quaternions valid (users being lazy) and unnormalised ones invalid (potentially sign of an error) |
Don't mix logic for what is considered a valid quat into this function; matches Ogre
Logic of what makes a quaternion valid isn't in normalizeQuaternion; also gets the invalid quaternion value logged to console
|
@rhaschke thank you for kick starting this and for giving permission for this branch to be adapted by us. As I mentioned, this PR now addresses the remaining places in the codebase @rhaschke identified where quaternions may be null/unnormalised. It addresses the issue that initially prompted rejecting invalid quaternions (#1137) and potentially others. It therefore removes the need for us to reject invalid quaternions (we now give warnings instead: #1182) I've minimised the diff to simplify review, including removing printing of warnings for invalid quaternions that are covered by #1182. @rhaschke The previous state of this PR had map_display rejecting null quaternions which is no longer the case since I imagine it was unintentional, please correct me if that's not the case |
|
@ros-pull-request-builder retest this please |
|
@ros-pull-request-builder retest this please (issues with the git refs) |
|
Obviously need to get the CI passing or verify it locally, otherwise lgtm. |
There was a problem hiding this comment.
+1
I my proposal, I tried to avoid computing the quaternion norm twice (once for validation and once for normalization). Hence the boolean return value of normalizeQuaternion().
The current proposal explicitly separates validation and normalization which is fine if the additional overhead is accepted. As I understood @tfoote in #1179 (comment), he wanted to reduce the computational overhead of normalization to a minimum ;-)
src/rviz/validate_quaternions.h
Outdated
| if ( 0.0f == x && 0.0f == y && 0.0f == z && 0.0f == w ) | ||
| { | ||
| w = 1.0f; | ||
| x = y = z = 0.0f; |
There was a problem hiding this comment.
This line is superfluous. x,y,z are zero already.
There was a problem hiding this comment.
gah, thanks for pointing that out: df6dfd4
src/rviz/validate_quaternions.h
Outdated
| if ( 0.0 == x && 0.0 == y && 0.0 == z && 0.0 == w ) | ||
| { | ||
| w = 1.0; | ||
| x = y = z = 0.0; |
src/rviz/validate_quaternions.h
Outdated
| template<typename T> | ||
| inline bool validateQuaternions(const T &vec) | ||
| inline bool validateQuaternions( const std::vector<T> &vec ) |
There was a problem hiding this comment.
I more like the old version as it is more compact and easier to grasp ;-)
There was a problem hiding this comment.
yeah, I know, but I'm trying to keep unnecessary changes out for this PR 😃
Yeah, for most of these msgs they're not being normalised elsewhere so it's roughly the same overhead as the validateQuaternions + tf normalisation in other places The devel jobs are behaving now so we'll just wait for that to turn over. Appreciate your contributions/insight on this @rhaschke |
|
I think we should reject quaternions that are not valid in the future. I'm in favour of:
It will take some time to fix the code base but the effort is worth it. |
|
Did someone test the overhead of this patch? eg: when displaying a large number of markers? |
|
OK, looks like we're all in agreement. Summarising the thread: invalid quaternions will be permitted for now to not break displays; publishers of invalid quaternions should be reported/updated; invalid quaternions will be rejected in the future.
Pretty much as you said, @VictorLamoine. This is the timeline to get to the end goal of not normalising inputs:
Regarding the overhead, we can consider adding an option to displays to assume valid quaternions if there are use cases for which the overhead needs to be avoided. |
|
@dhood @VictorLamoine In order to enforce valid quaternions on the generating side, you should provide tools to:
|
That's not possible because there are no default values for generated messages in ROS atm. In ROS 2 we have this (specifically for this case with quaternions), but I don't think there are plans to back port it, but with some community interest and help it might happen. |
Actually, I was afraid of this. For me, that's a clear reason that generation-side normalization is currently not feasible in a comfortable fashion. |
|
That's always been the case though. I don't see why requiring people to initialize their quaternions is a problem. Also for the special case of all zeros, we can continue to change that to identity for people, since that's not as expensive as always normalizing. That case, in my mind, is separate from giving unnormalized quaternions. |
src/rviz/validate_quaternions.h
Outdated
| x /= norm2; | ||
| y /= norm2; | ||
| z /= norm2; | ||
| float invnorm = 1.0f / norm2; |
There was a problem hiding this comment.
What's the benefit of this? Multiplication and division have identical costs on the CPU, haven't they?
Agreed. |
This is a follow-up to #1167, which added tests for invalid quaternions that caused crashes in Ogre.
However, instead of rejecting those invalid quaternions, I propose to normalize them (or use identity if a zero quaternion is passed). Actually this was already the default behaviour of
FrameManager::transform()which is used by most routines anyway.However, in rare cases ROS quaternions were used directly to set an
Ogre::Quaternion, which then indeed requires normalization and special handling of the zero quaternion.This PR reverts most changes introduced in #1167 and thus fixes the regression not handling non-normalized quaternions gracefully: #1178, moveit/moveit#732.