Robotics Stack Exchange is a question and answer site for professional robotic engineers, hobbyists, researchers and students. It only takes a minute to sign up.
Sign up to join this community
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.
Originally posted by Dragonslayer on ROS Answers with karma: 574 on 2020-11-21
Post score: 1
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?
transformations.py can help if using python, for C++ look at tf2::LinearMath::Matrix3x3
http://docs.ros.org/en/noetic/api/tf2/html/Matrix3x3_8h.html
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;
rot.getRotation(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.
https://math.stackexchange.com/questions/13150/extracting-rotation-scale-values-from-2d-transformation-matrix 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.
Originally posted by lucasw with karma: 8729 on 2020-11-22
This answer was ACCEPTED on the original site
Post score: 1
Comment by Dragonslayer on 2020-11-23:
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?
Comment by lucasw on 2020-11-23:
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.
Comment by lucasw on 2020-11-23:\
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.
Comment by Dragonslayer on 2020-11-23:
Thank you very much @lucasw ! You have been a lot of help, appreciate it.
