Remove many extra conversions from Matrix3x3 to Quaternion #741
Conversation
tf2/include/tf2/buffer_core.h
Outdated
| bool setTransformImpl( | ||
| const tf2::Transform & transform_in, const std::string frame_id, | ||
| const std::string child_frame_id, const TimePoint stamp, | ||
| const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id, |
There was a problem hiding this comment.
note: I changed the string values into references - this really should be a std::string_view - it shouldn't break any code, and ROS is c++17 now - but it's somewhat a moot point as SSO will kick in for most char* conversions.
|
@kscottz - tagging as requested |
|
Note: I've broken all tests, I'll fix it - the base idea is sound, however |
|
Never mind - appears to just be tests using hardcoded quaternion values, rather than actually checking equality - I'm happy to fix this either by manually fixing the test quaternions, or by doing some nicer equality check. This does mean that downstream users will sometimes get out different (but equiv) rotations from the library than they got before. |
|
Pushed what should hopefully fix the failing tests. |
Co-authored-by: jmachowinski <[email protected]> Signed-off-by: kyle-basis <[email protected]>
|
Hit a failed test in CI, I don't think this is my fault - ci flake? Locally: |
|
Sorry about the delay, had a power outage this morning. I'll take a look but I am not super familiar with the internals of TF / Geometry. Thanks for sticking with this and trying to get it over the line. We really appreciate the help and could always use a few more contributors. ❤️
I figured you might hit a few flakey tests as anything that relies on float / double ends up being a bit brittle. I can't speak to any of these tests being historically flakey. I think setting the epsilons to five or six sig figs should be sufficient.
Absolutely. We're starting to have conversations about bringing in someone to address API doc deficiencies but that's still far off. I would need more familiarity to do it myself.
This one's on you and what you're up for. I would have to look at the individual tests to make a call. Please do keep in mind that more you change on the lower level code the more likely it is that it may break something up stream. Smaller deltas are always better and get finished faster. You can always circle back
I'll need to look at these a bit more but probably warrants an issue with a |
For what it is worth, we have no reports of flakey tests from our nightly CI in this package that I can remember, and nothing is reported in https://github.com/ros2/geometry2/issues . So while it is always possible that there is a flakey test here, we do not currently know about any. |
| EXPECT_NEAR(q_simple.quaternion.z, -1 * M_SQRT1_2, EPS); | ||
| EXPECT_NEAR(q_simple.quaternion.w, 0, EPS); | ||
| { | ||
| const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( |
There was a problem hiding this comment.
It might help the reviewers, and readability, if you articulate what's going on in this function call. I've been looking at this for ten minutes and it still isn't quite clear what's going on.
There was a problem hiding this comment.
I added an intermediate variable - I think the diff moved your comment though, which line specifically is confusing?
There was a problem hiding this comment.
Eh, that doesn't really tell me what the test does, it just tells me what the result should be. What would really help readers is something like, // Frame a and frame b differ by [x,y,z] and [p,q,r] at time t, check the resulting quat is <foo>
That's why I dislike this big ball of wax test pattern, when you write one function per test you can name the tests something sane like, "check_basic_frame_transform" and it all makes sense. The original author really should have at least left us a few bread crumb comments. 😢
Anyway, not your fault, thanks for putting up with this stuff.
|
Sorry about that - done |
|
|
|
Fixed. Will work on perf testing today or tomorrow. |
|
Will fix the ...should I make a PR to fix the spelling of that warning? Edit: these warnings don't appear to be my fault. Edit2: the warnings were in fact my fault |
|
@kscottz I wrote a benchmark: #include <benchmark/benchmark.h>
#include "tf2_ros/buffer.h"
static void benchmark_set_transform(benchmark::State & state)
{
rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
buffer.setUsingDedicatedThread(false);
geometry_msgs::msg::TransformStamped transform;
transform.header.frame_id = "foo";
transform.header.stamp.sec = 0;
transform.header.stamp.nanosec = 0;
transform.child_frame_id = "bar";
transform.transform.translation.x = 42.0;
transform.transform.translation.y = -3.14;
transform.transform.translation.z = 0.0;
transform.transform.rotation.x = 0.591922;
transform.transform.rotation.y = 0.210684;
transform.transform.rotation.z = -0.762476;
transform.transform.rotation.w = 0.154502;
for (auto _ : state) {
transform.header.stamp.nanosec++;
buffer.setTransform(transform, "benchmark");
}
}
BENCHMARK(benchmark_set_transform);before: after This was run on a Jetson Orin kit, with cpu scaling governer set to |
|
Does this not all go away if we use a quaternion for rotation in Transform instead of a matrix? The Transform interface can remain identical -- getBasis() converts a quaternion to matrix instead. |
|
That would be even faster but constitute an API break I believe. How does rolling compare to this branch? This should also result in less copies/conversions but maybe my benchmark is bad.Sent from my iPhoneOn Jan 13, 2025, at 6:12 PM, Terry Scott ***@***.***> wrote:
I was curious about this myself. I replaced the Matrix in tf2::Transform with a Quaternion. 50% speed up in setTransform! (~120ns 😅 )
lookupTransform largely unaffected
see benchmarks in (jazzy / rolling )
—Reply to this email directly, view it on GitHub, or unsubscribe.You are receiving this because you were mentioned.Message ID: ***@***.***>
|
|
While I haven't looked at the details of this exact change, I did want to mention that API breaks are allowed in Rolling. However, if we do this, we'll need to keep the old API with a deprecation warning, and then add in a new API. |
|
I’m not quite sure how one would deprecate a class member like that.Sent from my iPhoneOn Jan 14, 2025, at 12:00 AM, Chris Lalancette ***@***.***> wrote:
While I haven't looked at the details of this exact change, I did want to mention that API breaks are allowed in Rolling. However, if we do this, we'll need to keep the old API with a deprecation warning, and then add in a new API.
—Reply to this email directly, view it on GitHub, or unsubscribe.You are receiving this because you were mentioned.Message ID: ***@***.***>
|
|
@ahcorde Can you start a CI ? |
|
ahcorde
left a comment
ahcorde
left a comment
There was a problem hiding this comment.
We still need to deprecated old headers
I'm confused, what headers are you referring to ? |
I'm refering to this comment |
|
This patch currently does not break API. Therefore there is no header to deprecate. The discussion was about a more substantial change that would yield a bigger performance gain. |
|
ups, I was lost in all the conversation, my bad. Approved |
|
🎉 Good work @kyle-basis! Thanks for your patience. We really appreciate the improvement. |
|
@scott-robotics can you post the results of your benchmark for Rolling or point me to the results? |
|
@kscottz I ran some benchmarks, building off what @kyle-basis has done. This PR has the same performance impact on Rolling (with this PR merged): Return Quaternion by reference ( scott-robotics:use-quaternion-as-basis-rolling, prior to this PR merge): |
7 participants
When calling
setTransform(), rotation data would flow asros_msgs::Quaternion->tf2::Quaternion->tf2::Matrix3x3, then reconvert totf2::Quaternion12 times to check validity of the rotation, then do one final conversion to actually store it inside the buffer.When calling
lookupTransform(), we'd go fromtf2::Quaternion->tf2::Matrix3x3->tf2::Quaternion(x4)->ros_msgs::Quaternion.I've fixed this in two parts -
setTransformImplandlookupTransformImplto take a vector+quat, eliminating both the roundtrip throughtf2::Transfromand the possible footgun ofgetRotationtf2::Transform, only callgetRotation()once, and reusing the result afterwardslookupTransformnorlookupVelocity, those are far less mechanical changes - would appreciate if someone fixed these up in a followup PRSome notes:
getRotation()is poorly named - would recommend deprecating and renaming tocalculateRotationor similar. I shot myself in the foot implementingtf2_basisand not realizing that origin is by reference and rotation is by value.tf2::Transformshould really be a vector+quat, but it would be an API change