Ask Your Question

openCV transform datatypes to ROS msg tf/goal

asked 2020-11-21 07:55:22 -0500

Dragonslayer gravatar image

updated 2020-11-22 09:35:24 -0500

lucasw gravatar image

Hi, Iam looking for the best practice to make a openCV transform (getAffineTransform() ) into a pathplanner goal message (angle, translation). I dont really understand all the different datatypes used. Help would be appreciated.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2020-11-22 10:09:14 -0500

lucasw gravatar image

updated 2020-11-23 11:54:30 -0500

This is mostly a math/opencv question, the translation and angle in the ros message could be a translation and angle for anything- but there are some ros tf classes and methods that can help (I can update this with more details later, or maybe someone else can help out).

It looks like the affine matrix is of the form

a b tx
c d ty

I think you can convert the 2x2 part of the 2x3 affine matrix into a 3x3 rotation matrix then extract a quaternion which should plug into your goal message (MoveBaseGoal or PoseStamped? It's not super important which it is because they both use quaternions).

a b 0
c d 0
0 0 1

But the affine matrix isn't necessarily a good rotation matrix? So the underlying rotation matrix code may force it to conform or blindly try to use it without checking and bad results will come out later in the process? can help if using python, for C++ look at tf2::LinearMath::Matrix3x3

You could create a Matrix3x3 with the row major formatting constructor using those abcd components above, then call getRotation to get a Quaternion out, then put the quaternion elements into your message:

tf2::Matrix3x3 rot(
    a, b, 0,
    c, d, 0,
    0, 0, 1);

tf2::Quaternion quat;

my_transform.rotation.x = quat.getX();
my_transform.rotation.y = quat.getY();
my_transform.rotation.z = quat.getZ();
my_transform.rotation.w = quat.getW();

The translation is the last column of the affine matrix, just leave z 0.0 since you only have x and y. looks helpful.

You may also need to look into how to pull values out of an opencv matrix elsewhere if you know nothing at all about the cv Mat datatype and are using C++. #q358188 has an example of this and shows includes for the tf2 data types.

edit flag offensive delete link more


Thanks for the answer. Yes Iam looking for a C++ solution. As also PCL and likely much more librarys use matrix transforms (for scientific reasons) I thought that its very likely that there is already ROS functionality that takes care of the data conversion, but this doesnt seem to be the case. I found this: eigen-convert-matrix3d-rotation-to-quaternion and eigen space Transformations

To me this looks like casting one to the other and Iam not sure how this would work or what to look out for, red also that data needs to be normalized before such operations (some say important others say doesnt really matter). Any thoughts on this?

Dragonslayer gravatar image Dragonslayer  ( 2020-11-23 08:27:55 -0500 )edit

I filled in some details above with the C++ route that uses the most ros tf code. It's possible to do it in eigen (which is probably happening underneath the ros tf classes anyhow), and probably the multiple different routes (in addition to tf1 vs tf2) contributes to it not being documented very well.

lucasw gravatar image lucasw  ( 2020-11-23 09:42:22 -0500 )edit

data needs to be normalized

I think your affine matrix could be a very poor rotation matrix, the tf2 Matrix3x3 + getRotation solution above could try to do something reasonable to normalize, or you could get an unnormalized quaternion out of it? I think you'll need to experiment there.

lucasw gravatar image lucasw  ( 2020-11-23 09:48:06 -0500 )edit

Thank you very much @lucasw ! You have been a lot of help, appreciate it.

Dragonslayer gravatar image Dragonslayer  ( 2020-11-23 09:59:28 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2020-11-21 07:55:22 -0500

Seen: 150 times

Last updated: Nov 23 '20