-
-
Save ashirsat/d7137b6911a21fea49e8e222f5dba848 to your computer and use it in GitHub Desktop.
ROS quaternion to rotation matrix OR rotation matrix to roll-pitch-yaw OR roll-pitch-yaw to quaternion
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <iostream> | |
#include <tf/tf.h> | |
int main(){ | |
/**< Declaration of quaternion */ | |
tf::Quaternion q; | |
q.setW(1); | |
q.setX(0); | |
q.setY(0); | |
q.setZ(0); | |
tf::Quaternion q2(0, 0, 0, 1); // x, y, z, w in order | |
/**< quaternion -> rotation Matrix */ | |
tf::Matrix3x3 m(q); | |
tf::Matrix3x3 m2; | |
m2.setRotation(q); | |
/**< rotation Matrix - > quaternion */ | |
m.getRotation(q); | |
/**< rotation Matrix -> rpy */ | |
double roll, pitch, yaw; | |
m.getRPY(roll, pitch, yaw); | |
/**< rpy -> quaternion */ | |
tf::Quaternion q3; | |
q3.setRPY(roll, pitch, yaw); | |
q3.normalize(); | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment