项目中需要将三维空间方向向量转化为旋转矩阵来表示,解决方案记录如下
基于Eigen库实现算法中的矩阵运算。
输入
方向向量Vector3d tmpvec,参考单位向量Vector3d zaxis(0,0,1)
输出
旋转矩阵m
Eigen::Matrix3d m;Eigen::Vector3d zaxis(0, 0, 1);Eigen::Vector3d tmpvec(vx, vy, vz);tmpvec.normalize();Eigen::Vector3d xaxis = zaxis.cross(tmpvec);xaxis.normalize();Eigen::Vector3d yaxis = tmpvec.cross(xaxis);yaxis.normalize();m(0, 0) = xaxis(0);m(0, 1) = yaxis(0);m(0, 2) = tmpvec(0);m(1, 0) = xaxis(1);m(1, 1) = yaxis(1);m(1, 2) = tmpvec(1);m(2, 0) = xaxis(2);m(2, 1) = yaxis(2);m(2, 2) = tmpvec(2);
得到旋转矩阵后便可基于EigEigen库计算四元数或欧拉角,根据参考向量的不同需要对旋转矩阵做相应调整,可参考[3]。
旋转矩阵