1
2
3
4
5
6
//转四元数
Eigen::Matrix3d rotationMat;
const Eigen::Quaterniond quat(rotationMat);
//转旋转矩阵
Eigen::Matrix3d Matrix_= quat.toRotationMatrix();

1
2
3
4
5
6
7
Eigen::Matrix4d outputMatrix;
Eigen::Matrix4d m4d=Eigen::Matrix4d::Identity();
Eigen::Matrix3d Matrix_= quat.toRotationMatrix();
outputMatrix.topLeftCorner(3, 3) = Matrix_;
outputMatrix.topRightCorner(3, 1) = m4d.topRightCorner(3, 1);
outputMatrix.bottomRightCorner(1, 4) << 0, 0, 0, 1;

1
2
3
4
5
Eigen::Matrix4d CalcoarseMatrix;
Eigen::Isometry3d isometry3dCoarse=Eigen::Isometry3d::Identity();
isometry3dCoarse.rotate(CalcoarseMatrix.block<3,3>(0,0));
isometry3dCoarse.pretranslate(Eigen::Vector3d(CalcoarseMatrix(0,3),CalcoarseMatrix(1,3),CalcoarseMatrix(2,3)));

1
2
3
Eigen::Matrix4f Transformation;
Eigen::Matrix4d Transformationd=Transformation.cast<double>();