Skip to content

Instantly share code, notes, and snippets.

@LimHyungTae
Created December 16, 2019 15:41
Show Gist options
  • Save LimHyungTae/39ecf62d7aee582791bd81872d8ea946 to your computer and use it in GitHub Desktop.
Save LimHyungTae/39ecf62d7aee582791bd81872d8ea946 to your computer and use it in GitHub Desktop.
ROS difference btw setRotation() and getRotation() in tf::Matrix3x3
#include <iostream>
#include <tf/tf.h>
void coutQuaternion(tf::Quaternion q){
std::cout<<q.getX()<<", "<<q.getX()<<", "<<q.getZ()<<", "<<q.getW()<<std::endl;
}
void coutTfMat(const tf::Matrix3x3& mat){
std::cout<<mat[0][0]<<mat[0][1]<<mat[0][2]<<std::endl;
std::cout<<mat[1][0]<<mat[1][1]<<mat[1][2]<<std::endl;
std::cout<<mat[2][0]<<mat[2][1]<<mat[2][2]<<std::endl;
}
int main(){
tf::Quaternion q_test(0.9698573, 0.0, 0.0242464, 0.2424643);
tf::Matrix3x3 mat(q_test);
coutTfMat(mat);
// [ 0.9988242, -0.0117578, 0.0470312;
// 0.0117578, -0.8824221, -0.4703116;
// 0.0470312, 0.4703116, -0.8812463 ]
tf::Quaternion q_test2(0.3697843, -0.9244608, 0.0092446, 0.0924461);
// [ -0.7094265, -0.6854115, -0.1640885;
// -0.6819930, 0.7263482, -0.0854628;
// 0.1777626, 0.0512777, -0.9827365 ]
std::cout<<"Before: ";
coutQuaternion(q_test2);
mat.setRotation(q_test2);
std::cout<<"After setRotation(): ";
coutQuaternion(q_test2);
coutTfMat(mat);
mat.getRotation(q_test);
std::cout<<"After getRotation(): ";
coutQuaternion(q_test);
return 0;
}
// Result in terminal...
//
// 0.998824-0.01175780.0470311
// 0.0117578-0.882422-0.470312
// 0.04703110.470312-0.881246
// Before: 0.369784, 0.369784, 0.0092446, 0.0924461
// After setRotation(): 0.369784, 0.369784, 0.0092446, 0.0924461
// -0.709427-0.685411-0.164089
// -0.6819930.726348-0.0854628
// 0.1777630.0512777-0.982737
// After getRotation(): -0.369784, -0.369784, -0.0092446, -0.0924461
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment