|
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 how C++ works, you can not do this:
|
1 |
Transform t = AngleAxis(angle,axis); |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity(); // Define a translation of 2.5 meters on the x axis. transform_2.translation() << 2.5, 1.0, 0.5; // The same rotation matrix as before; tetha radians arround Z axis transform_2.rotate (yawAngle*pitchAngle *rollAngle ); std::cout<<transform_2.matrix() <<std::endl; std::cout<<transform_2.translation()<<std::endl; std::cout<<transform_2.translation().x()<<std::endl; std::cout<<transform_2.translation().y()<<std::endl; std::cout<<transform_2.translation().z()<<std::endl; |
and with KDL Frame:
|
1 2 3 4 5 |
KDL::Frame F; F.M=F.M.RPY(roll, pitch, yaw); std::cout<<F.M(0,0) <<" " <<F.M(0,1)<<" " <<F.M(0,2) <<std::endl; std::cout<<F.M(1,0) <<" " <<F.M(1,1)<<" " <<F.M(1,2) <<std::endl; std::cout<<F.M(2,0) <<" " <<F.M(2,1)<<" " <<F.M(2,2) <<std::endl; |
