从旋转向量到旋转矩阵的转化公式

 这个博客是公式详解:

 https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html

 

Eigen::Matrix3d constructMatrix(Eigen::RowVector4d rotationVector) { double x = rotationVector(0, 0); double y = rotationVector(0, 1); double z = rotationVector(0, 2); double angle = rotationVector(0, 3); Eigen::Matrix3d rotatinMatrix; rotatinMatrix(0, 0) = cos(angle) + x * x* (1 - cos(angle)); rotatinMatrix(0, 1) = x * y * (1 - cos(angle)) - z * sin(angle); rotatinMatrix(0, 2) = y * sin(angle) + x * z* (1 - cos(angle)); rotatinMatrix(1, 0) = z * sin(angle) + x * y * (1 - cos(angle)); rotatinMatrix(1, 1) = cos(angle) + y * y * (1 - cos(angle)); rotatinMatrix(1, 2) = -x * sin(angle) + y * z * (1 - cos(angle)); rotatinMatrix(2, 0) = -y * sin(angle) + x * z * (1 - cos(angle)); rotatinMatrix(2, 1) = x * sin(angle) + y * z * (1 - cos(angle)); rotatinMatrix(2, 2) = cos(angle) + z * z * (1 - cos(angle)); return rotatinMatrix; }