Roll, pitch, yaw using Eigen and KDL Frame
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 |
/* yaw: A yaw is a counterclockwise rotation of alpha about the z-axis. The rotation matrix is given by R_z |cos(alpha) -sin(alpha) 0| |sin(apha) cos(alpha) 0| | 0 0 1| pitch: R_y A pitch is a counterclockwise rotation of beta about the y-axis. The rotation matrix is given by |cos(beta) 0 sin(beta)| |0 1 0 | |-sin(beta) 0 cos(beta)| roll: A roll is a counterclockwise rotation of gamma about the x-axis. The rotation matrix is given by R_x |1 0 0| |0 cos(gamma) -sin(gamma)| |0 sin(gamma) cos(gamma)| It is important to note that R_z R_y R_x performs the roll first, then the pitch, and finally the yaw Roration matrix: R_z*R_y*R_x */ double roll, pitch, yaw; roll=M_PI/3; pitch=M_PI/4; yaw=M_PI/6; Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); Eigen::Quaternion<double> q = yawAngle*pitchAngle *rollAngle; Eigen::Matrix3d rotationMatrix = q.matrix(); std::cout<<rotationMatrix <<std::endl; |
From Eigen documentation: If you are working with OpenGL 4×4 matrices then Affine3f and Affine3d are what you want. Since Eigen defaults to column-major storage, you can directly use the Transform::data() method to pass your transformation matrix to OpenGL. construct a Transform:
1 |
Transform t(AngleAxis(angle,axis)); |
or like this:
1 2 |
Transform t; t = AngleAxis(angle,axis); |
But note that unfortunately, because of […]
Roll, pitch, yaw using Eigen and KDL Frame Read More »