一、从旋转矩阵转四元数
旋转矩阵转四元数是一个相对简单的过程。
#include <Eigen/Dense> Eigen::Matrix3d rotation_matrix(3,3); Eigen::Quaterniond quaternion(rotation_matrix);
其中,Eigen::Matrix3d是3x3的旋转矩阵,Eigen::Quaterniond是4元数。将旋转矩阵作为参数传入Eigen::Quaterniond构造函数即可得到4元数。
二、四元数转矩阵
四元数转矩阵是四元数转旋转矩阵的第一步,它的实现相对简单。
Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();
其中,quaternion是表示旋转的四元数,rotation_matrix是得到的3x3的旋转矩阵。
三、旋转矩阵叉乘对应四元数
旋转矩阵叉乘对应四元数是四元数转旋转矩阵的第二步,是一个比较复杂的过程,需要使用到补数和叉积计算。
Eigen::Matrix3d rotation_matrix(3,3); Eigen::Vector3d axis(0.0, 0.0, 1.0); double angle = M_PI/2.0; rotation_matrix = Eigen::AngleAxisd(angle, axis); Eigen::Vector3d v(1.0, 0.0, 0.0); Eigen::Quaterniond q; q.setFromTwoVectors(v, rotation_matrix*v); Eigen::Matrix3d rotation_matrix_from_q = q.toRotationMatrix();
以上代码中,axis和angle表示旋转轴和旋转角度,rotation_matrix是根据axis和angle生成的旋转矩阵,v是待旋转的向量,q是通过计算得到的四元数。这段代码的作用是将v向量绕z轴旋转90度,并将旋转后的向量再次表示为一个3x3的旋转矩阵。
四、旋转矩阵的范数
旋转矩阵的范数表示旋转矩阵的大小,用来衡量旋转的大小,它的计算公式为:
||A|| = max│λ│
其中,A为一个矩阵,λ为A的特征值。
Eigen::Matrix3d A; double norm_A = A.norm();
以上代码中,A为一个3x3的旋转矩阵,norm_A表示矩阵A的范数。
五、旋转矩阵导数
求解旋转矩阵的导数是比较复杂的,需要使用到一些高级矩阵运算。
Eigen::Matrix3d A; Eigen::Matrix3d A_dot; Eigen::Matrix3d W; A_dot = A*W - W*A;
其中,A为一个3x3的旋转矩阵,W为一个3x3的旋转向量(这里采用的是轴角表示法)。A_dot为求解得到的旋转矩阵的导数。
六、旋转矩阵的二范数
旋转矩阵的二范数表示矩阵的特征值平方和的平方根,是一个基本的矩阵运算。
Eigen::Matrix3d A; double norm_A = A.norm(); double norm_A_squared = norm_A*norm_A;
以上代码中,A为一个3x3的旋转矩阵,norm_A表示矩阵A的范数,norm_A_squared表示二范数的平方。
七、旋转矩阵的导数选取
在求解旋转矩阵导数的过程中,如何选择W(旋转向量)是一个比较重要和复杂的问题。各种情况下,不同的W选择会有不同的效果。
这里以求解某一时刻的刚体角速度的求导为例,旋转矩阵R在这个刻度的变化速率为:
R_dot = w x R
其中,w为刚体角速度,x表示向量叉积。因此,可以选择旋转向量W为:
W = [w] = {{0, -wz, wy}, {wz, 0, -wx}, {-wy, wx, 0}}
Eigen::Matrix3d R; Eigen::Vector3d w; Eigen::Matrix3d W; Eigen::Matrix3d R_dot = W*R;
以上代码中,R表示旋转矩阵,w表示角速度,W为选择的旋转向量,R_dot表示R的变化速率。